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ABSTRACT 


This  study  aims  at  developing  an  improved  methodology  for  position  estimation 
by  which  sparse,  erroneous  and  inconsistent  sensor  observations  can  be  utilized  for 
Autonomous  Underwater  Vehicle  (AUV)  tenninal  homing  to  an  undersea  docking 
station. 

An  undersea  docking  station  offers  great  potential  for  increasing  AUV  at-sea  time 
and  reducing  survey  costs.  A  key  part  of  this  system  is  the  ability  of  the  AUV  to 
successfully  dock.  Part  of  the  process  for  successful  docking  is  AUV  position  estimation. 
The  Digital  Ultra-Short  Baseline  (D-USBL)  is  a  sensor  system  used  by  the  AUV  to 
provide  range  and  bearing  measurements  to  the  docking  station.  These  measurements  can 
be  used  by  the  AUV  to  improve  its  position  estimate. 

Due  to  the  nonlinearity  of  the  D-USBL  measurements,  the  Extended  Kalman 
Filter  (EKF),  Unscented  Kalman  Filter  (UKF)  and  forward  and  backward  smoothing 
(FBS)  filter  were  utilized  to  estimate  the  position  of  the  AUV.  After  perfonnance  of  these 
filters  was  deemed  unsatisfactory,  a  new  smoothing  technique  called  the  Moving  Horizon 
Estimation  (MHE)  with  epi-splines  was  introduced.  The  MHE  smoothing  filter  used  the 
dead  reckoning  measurements  and  the  D-USBF  measurements  as  constraints  in  the  epi- 
splines  optimization  method. 

The  analysis  based  on  data  sets  of  REMUS  100  AUV  docking  station  runs  was 
conducted  using  the  MHE/epi-splines  methodology  and  compared  to  EKF  and  UKF 
algorithms.  The  MHE/epi-splines  algorithm  demonstrated  significantly  better 
performance  over  the  EKF  and  UKF. 
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I. 


INTRODUCTION 


A.  MOTIVATION 

The  autonomous  underwater  vehicle  (AUV)  has  taken  center  stage  in  defense 
underwater  surveys.  AUVs  are  capable  of  doing  more  work  over  a  short  period  while 
maintaining  low  operational  costs.  As  of  2017,  AUVs  are  used  to  conduct  mine  hunting 
operations,  sea  floor  mapping  and  other  important  activities  previously  conducted  by 
divers.  In  the  civilian  environment,  AUVs  are  used  mostly  to  inspect  underwater  pipes 
for  maintenance  purposes.  Most  civilian  companies  invest  their  resources  into  ensuring 
they  have  the  best  AUV  with  the  latest  equipment  onboard  to  achieve  their  mission 
requirements  successfully.  The  challenge  facing  the  underwater  robotics  community  is 
the  cost  of  deploying  and  extracting  these  AUVs  under  potentially  bad  weather 
conditions.  AUVs  have  limited  operational  duration  due  to  their  rechargeable  battery 
capacity  and  their  data  storage  capacity.  An  undersea  docking  station  offers  great 
potential  for  increasing  at-sea  time  and  reducing  survey  costs  of  AUVs.  An  undersea 
docking  station  provides  the  AUV’s  battery  recharging  facility,  data  download  and 
offload  activities.  The  key  part  of  using  the  undersea  docking  station  is  the  ability  of  the 
AUV  to  dock.  Once  the  AUV  has  docked  in  an  undersea  docking  station,  the  batteries 
will  be  recharged  and  the  data  will  be  transferred  to  the  user  on  the  surface  stations  and 
the  vehicle  mission  will  be  updated.  The  use  of  an  undersea  docking  station  pennits  the 
AUV  to  remain  at  sea  for  extended  periods  of  time. 

B.  PROBLEM  STATEMENT 

The  purpose  of  this  study  is  to  develop  a  methodology  for  position  estimation  by 
which  sparse,  erroneous,  and  inconsistent  sensor  observations  can  be  used  for  AUV 
tenninal  homing  to  an  undersea  docking  station.  An  undersea  docking  station  offers  great 
potential  for  increasing  at-sea  time  and  reducing  survey  costs  of  AUVs.  A  key  part  of  this 
system  is  the  AUV’s  ability  to  successfully  dock.  Part  of  the  process  for  successful 
docking  is  estimating  the  AUV’s  position.  The  AUV’s  digital  ultra-short  baseline  (D- 
USBL)  sensor  system  provides  range  and  bearing  measurements  to  the  docking  station. 
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The  D-USBL  range  and  bearing  measurements  are  nonlinear,  sparse,  erroneous  and 
inconsistent. 

Figure  1,  courtesy  of  Horner  and  Mqana  [1],  shows  a  REMUS  100  AUV  with  a 
Woods  Hole  Oceanographic  Institution  (WHOI)  docking  station  used  by  the  Naval 
Postgraduate  School  (NPS)’s  Center  for  Autonomous  Vehicle  Research  (CAVR).  The 
docking  station  is  mounted  on  the  ocean  floor  in  the  vicinity  of  the  Monterey  Inner  Shelf 
Observatory  in  Monterey  Bay,  California.  The  image  shows  the  REMUS  100 
successfully  docked  at  the  docking  station.  The  docking  station  has  a  vertical  USBL 
transponder  affixed  to  the  top,  a  big  yellow  cylindrical  tank  housing  a  9.6  kWh  battery 
pack,  and  a  camera  next  to  the  USBL  transponder.  The  vehicle  is  prohibited  from 
traveling  under  the  docking  station  by  plexiglass,  shown  as  the  black  object  at  the  bottom 
of  the  image  [1]. 


Figure  1.  NPS  REMUS  100  with  WHOI  Docking  Station.  Source:  [1]. 

The  AUV  starts  the  terminal  homing  phase  in  an  undersea  docking  station 

operation  on  the  sea  surface  with  access  to  Global  Positioning  System  (GPS) 
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measurements.  As  the  vehicle  dives  deeper  underwater,  it  loses  the  GPS  fixes  and  relies 
on  the  measurements  received  from  dead  reckoning  sensors  such  as  Doppler  Velocity 
Log  (DVL),  Inertial  Navigation  System  (INS),  Depth  sensor  and  Inertial  Measurement 
Unit  (IMU).  These  sensors  have  various  sampling  rates  not  exceeding  10Hz  and  the 
position  uncertainty  grows  linearly  over  distance  travelled.  In  order  to  minimize  the 
navigation  error  as  the  AUV  begins  the  docking  sequence,  the  AUV  is  installed  with  a  D- 
USBL  sensor  on  the  bow  and  the  docking  station  is  installed  with  USBL  transponder. 
The  AUV  initiates  an  interrogation  of  the  docking  station  transponder  and  it  continues  to 
send  a  periodic  sound  pulse.  When  the  AUV  is  within  range  it  may  receive  a  response 
and  transform  it  to  range  and  bearing.  The  range  and  bearing  measurements  are  then  used 
with  dead  reckoning  measurements  to  update  the  position  of  the  AUV.  The  challenge  in 
position  estimate  for  the  AUV  is  how  to  properly  incorporate  these  infrequent,  potentially 
erroneous  and  inconsistent  measurements  into  the  position  estimation  filter. 

This  study  aims  at  developing  a  methodology  that  can  utilize  the  sparse, 
erroneous  and  inconsistent  USBL  measurements  to  guide  the  AUV  to  an  undersea 
docking  station.  The  USBL  range  and  bearing  measurements  are  nonlinear  in  terms  of  the 
process  used  to  compute  them  and  in  tenns  of  their  inconsistency.  Due  to  nonlinearity  of 
the  USBL  range  and  bearing  measurements,  the  nonlinear  position  estimating  filters 
such  as  Extended  Kalman  Filter  (EKF),  Unscented  Kalman  Filter  (UKF)  and 
forward  and  backward  smoothing  (FBS)  are  considered  as  possible  solutions  and 
evaluated  using  previous  REMUS  100  AUV  docking  station  runs.  Additionally,  a 
modified,  Moving  Horizon  Estimation  (MHE)  position  estimating  method  is  developed 
and  compared  with  the  alternative  methods.  The  MHE  approach  is  novel  due  to  the  use 
of  a  non-linear  least  squares  optimization  approach  known  as  epi-splines.  Results 
between  the  methods  are  compared  and  contrasted  using  simulated  and  in-water  results. 

C.  LITERATURE  REVIEW 

1.  REMUS  100  Vehicles 

The  REMUS  100  vehicle  was  first  used  for  coastal  monitoring  missions,  but 
recently,  it  has  been  used  in  many  underwater  operations  by  both  defense  and  civilian 
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industries  [2].  REMUS  100  is  an  AUV  designed  to  operate  within  100  m  of  the  ocean’s 
surface.  REMUS  100  is  cylindrical  in  shape  and  portable  in  size,  so  it  can  be  carried  by 
two  persons.  The  modular  design  of  the  REMUS  100  ensures  relatively  easy 
interchanging  of  components  depending  on  mission  requirements. 

Current  designs  come  with  the  latest  technology  to  maintain  high  performance 
and  engineering  quality.  Although  the  size  of  the  instrumentation  has  decreased,  the 
vehicle’s  endurance  and  perfonnance  has  increased.  REMUS  100  vehicles  were 
originally  designed  by  the  Woods  Hole  Oceanographic  Institution  (WHOI)  lab  in  the 
United  States.  The  technology  was  further  developed  by  Hy droid,  Inc.  and  this  company 
was  eventually  purchased  by  Kongsberg  Maritime  of  Norway  [3].  These  vehicles  are 
attractive  to  users  because  they  are  relatively  inexpensive  and  light-weight  as  well  as 
easily  operated  by  a  trained  operator  via  a  standard  laptop  computer. 

2.  Underwater  Docking  Stations 

In  the  past,  underwater  missions  were  short  and  quick.  Now,  with  advanced 
technology,  underwater  missions  can  be  as  long  as  the  user  desires.  The  new  AUVs  have 
better  instrumentation,  which  produces  high-quality  results,  and  conduct  longer 
operations.  The  latest  method  for  ensuring  that  the  underwater  environment  is  well- 
known  by  the  users  is  to  keep  the  AUV  underwater  and  ready  for  the  next  operation. 
Underwater  docking  stations  help  ensure  that  the  vehicle  is  kept  underwater  with  charged 
batteries  and  with  an  updated  mission  plan. 

Underwater  docking  stations  are  connected  to  the  base  station  by  a 
communication  cable  that  allows  the  vehicle  to  recharge  its  batteries  and  transfer  data. 
Underwater  docking  stations  save  time  for  deploying  the  vehicle  and  retrieving  it  after 
the  mission.  They  also  save  time  transferring  data  and  loading  the  new  mission.  The 
underwater  docking  stations  also  help  to  increase  the  mission  time. 

Most  underwater  docking  stations  are  tapered  in  shape,  or  funnel  shaped,  to  catch 
the  cylindrical  vehicle  when  it  misses  the  center  of  the  docking  station.  The  conical  shape 
can  complicate  the  docking  event  because  the  AUV  can  approach  the  docking  station 
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from  only  one  direction  unless  the  entrance  cone  pivots  around  a  central  axis,  B.  W. 
Hobson  et  al.  [4], 

B.  W.  Hobson  et  al.  [4].  at  the  Monterey  Bay  Aquarium  Research  Institute 
(MBARI)  developed  and  used  the  underwater  docking  station  for  docking  the  institute’s 
twenty  one  inch-diameter  AUV.  MBARI’s  docking  station  is  shown  in  Figure  2.  This 
underwater  docking  station  has  had  many  successful  docking  events,  which  included 
transferring  data,  recharging  batteries,  and  completing  the  power  cycle  of  the  AUV. 


Figure  2.  MBARI  AUV  Docking  Station.  Source:  [4]. 


Another  interesting  underwater  docking  station  was  designed  by  C.  J.  von  Alt  et 
al.  [5].  The  cone  section  of  its  underwater  docking  station  is  slightly  different  from  the 
one  shown  in  Figure  2.  WHOI’s  underwater  docking  station,  like  MBARI’s,  can  transfer 
data  between  the  AUV  and  the  home  unit  as  well  as  recharge  the  AUV’s  batteries.  Figure 
3  shows  the  WHOI’s  underwater  docking  station. 
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Figure  3.  Complete  AUV  Docking  Station.  Source:  [5]. 


M.  D.  Feezer  et  al.  [6].  conducted  underwater  docking  operations  using  an 
electromagnetic  homing  system  to  estimate  the  optimal  position  and  orientation  of  the 
AUV.  M.  D.  Feezer  et  al.  installed  a  transmitter  on  the  underwater  docking  station  and  a 
receiver  on  the  AUV.  Figure  4  diagrams  how  the  AUV  interacts  with  the  underwater 
docking  station  when  the  electromagnetic  homing  system  is  used. 


Figure  4.  Diagram  of  an  Underwater  Docking  Station  with  Electromagnetic 

Field  Lines.  Source:  [6]. 

The  underwater  docking  stations  with  electromagnetic  homing  have  the  disadvantage  of 
having  to  handle  unknown  interference  with  nearby  metallic  objects  that  produce 
additional  magnetic  fields. 
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Some  docking  stations  are  equipped  with  both  optical  homing  systems  and 
acoustic  homing  systems  to  maximize  the  chances  of  obtaining  a  successful  docking 
event.  This  is  a  potentially  useful  approach  in  that  the  D-USBL  system  is  used  to  guide 
the  vehicle  from  farther  away  while  the  camera  system  can  be  used  to  provide  final 
guidance  commands.  In  this  study,  the  underwater  docking  station  is  equipped  with  only 
a  USBL  transponder  that  communicates  with  the  D-USBL  sensor  installed  in  the  REMUS 
100  vehicle. 


3.  Underwater  Docking  Environment 

As  touched  upon  in  the  previous  section,  the  undersea  environment  greatly 
impacts  the  sensor  perfonnance.  Common  sensor  modalities  include  vision  (optical) 
homing  and  acoustics  homing  methods.  The  vision  homing  method  is  known  for  its  high 
update-rate  advantage.  However,  the  vision  (optical)  docking  method  requires  relatively 
clear  water  visibility.  This  can  be  evaluated  with  the  REMUS  100  through  the  optical 
backscatter  sensor.  Baratoff  and  Blansteen  [7]  found  that  the  vision  homing  method  can 
also  be  degraded  by  ambient  light  and  infrared  radiation.  The  vision  homing  method  is 
only  effective  when  the  AUV  is  relatively  close  to  the  underwater  docking  station. 
Depending  on  water  clarity  it  is  limited  to  tens  of  meters. 

Figure  5  indicates  the  possible  enviromnent  during  underwater  docking  missions. 
The  sea  growth  can  obstruct  the  view  of  the  AUV  when  docking  using  the  optical  homing 
method. 


7 


Figure  5.  AUV  Approaching  the  Underwater  Docking  Station.  Source:  [7]. 

If  the  acoustic  homing  method  is  used,  the  sea  water  needs  to  be  quiet  to  allow  for 
clear  sound  propagation  between  the  transponder  mounted  in  the  docking  station  and  the 
D-USBL  in  the  AUV.  While  visibility  does  not  negatively  affect  the  acoustics  homing 
method,  proximity  to  the  sea  floor  or  subsea  structures  does.  Although  the  acoustic 
homing  method  is  affordable,  it  is  known  for  low  update  rates.  Figure  6  shows  the 
MBARI  underwater  docking  station  which  used  an  acoustic  homing  method  to  dock  the 
twenty  one  inch  Dorado  AUV. 
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Figure  6.  MBARI  Underwater  Docking  Station  in  Use.  Source:  [5]. 

In  some  cases,  the  vision  and  acoustic  guidance  methods  are  combined  to  improve 
the  docking  operation.  The  position  of  the  docking  station  for  this  combination  should  be 
optimized  to  ensure  that  both  methods  produce  usable  results,  A.  Myagotin  et  al.  [8]. 

4.  EKF  and  UKF  Position  Estimating  Methods 

This  study  evaluates  the  position  estimating  methods  on  using  the  nonlinear, 
sparse,  erroneous  and  inconsistent  USBL  measurements  to  guide  the  AUV  to  an  undersea 
docking  station.  Some  studies  are  particularly  relevant  to  this  thesis.  For  instance,  a  few 
publications  have  covered  the  case  of  tracking  underwater  vehicles  using  D-USBL 
measurements  and  the  optical  guidance  method.  Filip  Mandic  et  al.  [9].  used  D-USBL 
and  sonar  image  measurements  to  track  the  underwater  target  precisely.  Figure  7  depicts 
a  small  grayscale  image  captured  by  the  forward  looking  sonar  system.  D-USBL 
measurements  assist  in  bounding  the  uncertainties  of  the  image  to  identify  the  object. 
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Figure  7.  D-USBL  and  Sonar  Image  Positioning  System.  Source:  [9]. 

The  EKF  was  used  to  estimate  the  optimal  position  of  the  target  from  the 
availability  of  the  target  image  in  the  sonar’s  field  of  view.  This  study  was  conducted  in 
two  scenarios,  one  with  a  moving  vehicle  and  slowly  drifting  target  and  one  with  a  static 
vehicle  and  moving  target.  The  results  were  as  follows:  the  position  estimate  drifted 
every  time  the  target  was  outside  the  sonar’s  field  of  view  and  D-USBL  measurements 
were  unavailable.  The  fusion  of  the  two  sensors  produced  the  most  accurate  tracking 
results  in  both  scenarios. 

D-USBL  measurements  have  become  the  common  method  for  tracking  the 
position  of  targets  under  water.  N.  Stilinovic  et  al.  [10]  conducted  a  study  whereby  the  D- 
USBL  was  used  to  track  a  diver  under  water.  An  AUV  that  floated  on  the  surface 
provided  the  diver  below  with  correct  GPS  coordinates.  N.  Stilinovic  et  al.  investigated 
“the  guidance  and  control  algorithms  for  the  autonomous  surface  platfonn  which  enables 
diver  tracking”  [10].  In  the  study,  the  D-USBL  was  used  to  track  the  vertical  position  of 
the  diver  while  the  GPS  was  used  for  horizontal  positioning.  The  EKF  was  used  to 
provide  the  diver  position  estimates.  The  simulation  yielded  acceptable  results  except  for 
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when  the  diver  turned  faster  than  the  surface  platform,  thereby  causing  errors  at  the 
comers. 

Unlike  the  study  conducted  by  Mandic  et  al.  [9].,  N.  Stilinovic  et  al.  [10]  tracked 
the  underwater  moving  object  with  D-USBL  measurements  only.  The  study  by  Mandic  et 
al.  [9].  is  closer  to  the  current  study  because  measurements  from  two  sources,  sonar 
imaging  and  D-USBL  measurements,  were  fused  to  obtain  the  optimal  state  of  the 
underwater  object.  This  thesis  intends  to  apply  a  fusion  of  measurements  from  many 
AUV  built-in  sensors  such  as  the  DVL,  depth  sensor,  INS,  IMU  and  GPS  as  well  as  the 
D-USBL.  The  advantage  of  the  current  study  is  that  the  target  is  stationary  and  its 
position  is  known. 

0.  Hegrenaes  et  al.  [11]  discovered  another  method  of  improving  underwater 
navigation  and  control.  Hegrenaes  et  al.  discovered  the  method  called  underwater 
transponder  positioning  (UTP).  This  method  integrates  the  AUV’s  INS  with  the  range 
measurements  from  the  transponder.  The  UTP  method  is  similar  to  the  D-USBL  but 
experiences  better  accuracy  due  to  its  tight  integration  with  the  INS.  Hegrenaes  et  al.  used 
the  Kalman  filter  to  estimate  the  optimal  position  of  the  transponders  for  this  method. 
Hegrenaes  et  al.  discovered  that  the  UTP  aiding  method  is  possible  and  accurate  for 
large-scale  underwater  operations  [11].  Hegrenaes  et  al.  study  is  related  to  this  thesis 
because  it  used  UTP-aided  INS  sensor  measurements  to  bound  the  measurement  errors. 
This  thesis  intends  to  bound  the  built-in  sensor  measurement  errors  using  D-USBL  fixes 
when  they  are  available. 

Paul  Frontera  [12]  conducted  a  study  to  estimate  the  state  of  the  system  with 
infrequently  available  measurement  information.  Frontera’s  study  aimed  at  estimating  the 
submarine  position  without  accessing  the  information  from  landmarks.  Frontera  focused 
on  comparing  the  EKF’s  and  UKF’s  performances  in  handling  the  nonlinear  and  sparse 
D-USBL  measurements.  Frontera  discovered  that  the  EKF  and  UKF  show  similar 
performances  at  short  measurement  intervals,  but  the  UKF  performs  better  at  longer 
measurement  intervals.  These  findings  were  expected  as  the  UKF  is  adept  at  handling 
highly  nonlinear  measurements  while  the  EKF  uses  second-order  Taylor  series 


11 


approximation,  which  fails  at  highly  nonlinear  measurements.  This  thesis  intends  to 
explore,  through  simulation,  the  EKF  and  UKF  optimal  state  estimation  methods. 

Another  interesting  use  of  the  position  estimating  method  appears  in  a  study  by 
Mohammed  and  Alzubaidi  [13].  Mohammed  and  Alzubaidi  used  the  position  estimating 
method  to  track  an  attacking  missile  and  prevent  it  from  hitting  its  target  using  the  EKF. 
Mohammed  and  Alzubaidi  found  that  the  estimated  positions  of  the  target  were  more 
accurate  when  the  target  was  closer  to  the  object. 

The  position  estimation  method  is  also  used  in  missile  guided  systems,  as 
described  in  M.  S.  Bezick  et  al.  [14].  Bezick  et  al.  explored  the  “inertial  navigation  for 
guided  missile  using  the  position,  velocity  and  altitude  for  missile  guidance  and  control 
and  the  inflight  alignment  technique  that  can  be  used  to  increase  the  accuracy  of  the 
missile  navigation  system  data  by  incorporating  the  external  non-inertial  navigation- 
aiding  data  using  the  Kalman  filter  and  the  measurements  from  the  external  radar  track  of 
the  missile  and  the  GPS  receiver”  [14], 

Feezer  et  al.  [6].  used  the  electromagnetic  homing  system  to  bound  the  errors 
produced  by  the  AUV’s  dead  reckoning  systems  when  performing  the  underwater 
docking  operation.  The  electromagnetic  homing  method  provides  the  AUV’s  bearing  and 
orientation,  which  are  very  important  for  successfully  docking,  especially  when  the  AUV 
is  close  to  the  docking  station.  The  electromagnetic  homing  system  was  found  very 
effective  at  a  range  of  25  m  to  30  m  from  the  docking  station.  This  method  can  also  be 
used  for  multiple  targets  tracking  at  the  same  time.  The  tracking  method  in  [6]  works 
similar  to  the  optical  homing  method  explored  by  Mandic  et  al.  [9].  Both  methods 
provide  good  update  rates  but  only  at  closer  ranges. 

N.  Nadarajah  et  al.  [15]  conducted  a  study  to  improve  “the  estimates  for  a  multi 
target  tracking  problem  using  fixed-lag  smoothing”  [15].  Nadarajah  et  al.  performed  the 
fixed-lag  tracking  using  forward  filtering  followed  by  backward  smoothing.  The  standard 
Kalman  filter  performed  forward  filtering  in  fixed-lag  smoothing  while  the  information 
filter  performed  backward  smoothing.  Although  these  filters  use  different  sets  of 


12 


measurements,  they  should  converge  into  one  optimal  estimate.  Such  a  method  is 
explored  in  this  thesis  and  may  be  integrated  into  the  final  guidance  algorithm. 

The  EKF  was  also  used  to  estimate  the  optimal  position  and  attitude  of  spacecraft 
flying  in  fonnation.  This  study  was  conducted  by  S.  Kim  et  al.  [16].  Given  the  study’s 
success  in  tracking  moving  and  stationery  targets  using  position  estimating  methods,  it  is 
highly  possible  to  guide  an  AUV  to  a  stationary  underwater  docking  station  using  no  GPS 
measurements  and  relying  solely  on  sparse  D-USBL  fixes  to  bound  the  state  estimation 
errors. 


5.  Moving  Horizon  Estimation  Position  Estimating  Method 

Moving  horizon  estimation  (MHE)  is  the  estimation  method  that  considers  the 
constraints  of  vehicle  dynamics  while  maintaining  a  minimum  error  between  the  estimate 
and  actual  data.  Although  the  MHE  originated  in  the  1960s,  it  is  now  regarded  as  an 
important  technique  for  position  estimation  for  dealing  with  nonlinear  data  sampled  at 
multisampling  rates  with  outliers.  The  MHE  has  been  applied  in  many  different 
estimation  scenarios  by  Royset  and  Wets  [17]. 

The  advantage  of  the  MHE  over  the  other  estimation  techniques  is  that  it  uses 
constraints  to  ensure  the  estimation  results  are  not  influenced  by  outlier  measurements. 
This  property  forces  the  user  to  establish  constraints  from  the  beginning  of  MHE  usage. 
M.  W.  Mehrez  et  al.  [18]  used  the  MHE  to  estimate  the  position  of  multiple  robots  when 
the  initial  estimate  was  unknown.  Mehrez  et  al.  chose  the  MHE  over  the  well-known 
EKF  due  to  its  advantage  of  working  without  knowing  the  initial  estimate.  The  MHE 
performs  a  high  number  of  iterations  and  converges  more  quickly  to  the  solution  with  the 
accepted  error  range.  The  EKF  is  highly  affected  by  the  initial  conditions  and  may 
diverge.  The  study  by  Mehrez  et  al.  found  the  MHE  is  unaffected  by  any  kind  of 
distribution.  The  challenge  for  the  MHE,  however,  is  solving  linear  programming  at 
every  time  step.  In  Mehrez  et  al’s  the  study,  the  MHE  performed  better  when  the  initial 
position  estimate  of  the  robot  was  unknown.  The  MHE  uses  the  least  squares 
approximation  to  overcome  the  nonlinearity  of  the  measurements  and  to  obtain  prior 
knowledge  to  generate  effective  estimates  when  none  are  available. 
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The  level  of  errors  and  rate  of  convergence  produced  by  the  MHE  makes  it 
attractive  to  the  environment  of  noisy  sensors  and  limited  measurements  in  multi-rate 
systems.  Andong  and  Wen- An  [19]  defines  the  MHE  as  “an  optimization  approach  that 
produces  estimates  of  states  by  using  moving  limited  observed  information.”  Andong  and 
Wen-An  investigated  the  estimation  performance  of  the  MHE  on  a  system  that  produces 
measurements  at  different  sampling  rates.  The  study  employed  an  inertial  sensor  and 
camera  to  locate  the  position  of  a  mobile  robot.  The  MHE  was  used  to  merge  the 
information  from  these  two  sources  to  improve  the  estimation  results. 

The  goal  of  the  MHE  is  to  minimize  the  objective  function.  The  objective 
function  comprises  two  parameters,  the  arrival  cost  and  the  stage  cost.  M.  K. 
Ramalingam  [20]  defines  the  arrival  cost  as  the  uncertainty  in  the  a  priori  estimate. 
Approximating  the  arrival  cost  is  the  main  challenge  for  the  MHE.  The  arrival  cost  is 
computed  using  nonlinear  filters  and  passed  to  the  MHE  for  optimization.  If  the  nonlinear 
filter,  such  as  the  UKF,  produces  a  poor  estimate  of  the  arrival  cost,  the  MHE’s  initial 
optimization  will  also  be  poor.  M.  A.  Muller  [21]  introduced  the  MHE  scheme  that 
confirms  the  assumption  of  prior  weighting  in  the  cost  function  offline. 

The  MHE  can  be  applied  in  many  estimation  environments.  M.  Zanon  et  al.  [22] 
used  the  nonlinear  version  of  the  MHE  to  find  estimates  for  the  friction  coefficient  of 
autonomous  vehicles  travelling  on  the  road.  Zanon  et  al.  used  the  direct  multiple  shooting 
method  to  find  the  estimated  friction  coefficient  between  the  tires  and  the  road.  The  study 
used  least  squares  nonlinear  programming  as  a  mathematical  tool  to  execute  the 
computations  in  the  MHE  application.  Zanon  et  al.  also  used  the  real-time  iteration 
scheme  with  shifting  since  the  computational  effort  for  optimization  is  high  for  real-time 
feedback  control  purposes. 

According  to  Zanon  et  al.  [22],  “The  real  time  iteration  is  also  found  to  reduce  the 
latency  between  the  instant  when  the  measurements  are  available  and  the  instant  when 
the  feedback  is  available  by  preparing  the  computations  as  much  as  possible  even  before 
the  measurements  are  available.”  The  MHE  produced  estimates  for  the  friction 
coefficient  in  the  study  within  milliseconds  at  the  complexity  of  highly  nonlinear  models. 
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The  MHE  is  sometimes  used  with  epi-splines  as  a  nonlinear  least-squares  cost- 
function  minimization  technique. 

Epi-splines  were  also  used  by  Royset  and  Wets  [17]  in  the  article  entitled  “From 
Data  to  Assessments  and  Decisions:  Epi-Spline  Technology.”  Royset  and  Wets  defines 
epi-splines  as  “the  newly  developed  piecewise  polynomial  functions  described  by  the 
finite  number  of  parameters,  which  are  exceptionally  flexible.”  As  described  in  the 
article,  epi-splines  are  used  for  image  reconstruction,  the  probability  density  estimation  of 
simulation  output,  as  well  as  electricity  demand  forecasts  with  respect  to  weather 
conditions.  In  all  of  these  applications,  epi-splines  were  highly  effective  at  predicting 
results.  The  effectiveness  of  the  MHE  depends  on  the  mode  of  the  probability  density 
function  [17]. 


6.  Forward  and  Backward  Smoothing  Position  Estimating  Method 

The  forward  and  backward  smoothing  is  a  technique  applied  to  the  measurements 
to  obtain  an  optimal  smoothed  estimate,  as  described  in  D.J.  Simon  [23].  This  technique 
results  in  two  optimal  estimates.  The  first  estimate  is  obtained  using  an  ordinary  Kalman 
filter  running  forward  in  time  from  the  first  USBL  measurement  to  the  USBL 
measurement  at  the  median  of  the  measurements  interval.  The  second  estimate  is 
obtained  using  an  infonnation  filter  running  backward  in  time  from  the  last  measurement 
in  the  interval  to  the  measurement  at  the  median  of  the  measurements  interval.  D.J. 
Simon  [23]  states  that  two  estimates  are  unbiased  because  they  are  the  outputs  of  the 
filters.  The  two  estimates  are  combined  according  to  their  Kalman  gains. 

The  covariance  of  the  forward  smoothing  step  is  initiated  as  a  big  finite  value 
while  the  covariance  for  the  backward  smoothing  step  is  infinite  [23].  Since  the  infinite  is 
not  a  valid  number  for  computational  purposes,  the  infonnation  filter  is  used  for  the 
inverse  of  the  infinite  covariance.  The  information  filter  uses  zero  as  the  confidence  at  its 
starting  point.  A  zero  confidence  for  the  information  filter  means  there  is  no  knowledge 
of  the  state  at  the  starting  point.  For  computational  purposes,  the  certainty  of  the 
information  filter  is  initiated  at  small  numbers  greater  than  zero  [23]. 
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The  knowledge  of  the  state  at  the  information  filter  initialization  point  is  key  for 
better  performance  of  backward  smoothing.  Simon  states  that  if  the  position  of  the  object 
is  unknown  at  the  information  filter  starting  point,  the  state’s  initial  condition  should  be 
zero,  and  the  state  will  be  updated  only  by  observation  and  the  Kalman  gain.  This  could 
cause  the  information  filter  to  diverge  if  the  actual  state  is  far  away  from  the  information 
filter  starting  position. 

D.  STRUCTURE  OF  THE  THESIS 

This  thesis  is  divided  into  seven  chapters.  Chapter  II  provides  an  overview  of  the 
systems  used  in  this  thesis.  Chapter  III  contains  the  problem  description  and  simulated 
analysis  of  the  UKF  and  EKF  position  estimating  techniques.  Chapter  IV  contains  the 
detailed  description  of  the  Moving  Horizon  Estimation  (MHE)  with  epi-spline  algorithm. 
Chapter  V  presents  the  results  from  using  UKF,  EKF  and  MHE  on  previous  REMUS  100 
docking  station  runs.  Chapter  VI  contains  a  detailed  description  of  the  FBS  position 
estimating  method  as  well  as  analysis  results  whereby  the  FBS  is  evaluated  and  compared 
to  the  UKF,  EKF  and  MHE  position  estimating  techniques.  Chapter  VII  concludes  the 
paper. 
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II.  SYSTEM  OVERVIEW 


A.  NPS  REMUS  100  VEHICLE  DESCRIPTION 

The  NPS  REMUS  100  vehicle  has  an  aft  propeller  with  two  aft  through-body 
thrusters  (vertical  and  horizontal)  and  two  forward  through-body  thrusters  (vertical  and 
horizontal).  The  NPS  REMUS  100  vehicle  was  designed  and  manufactured  by  Hy droid  and 
can  operate  to  a  maximum  depth  of  100  m.  REMUS  100  vehicles  are  equipped  with 
propellers  and  fins  for  moving  forward  and  for  diving.  The  main  navigational  sensors 
installed  in  the  NPS  REMUS  100  vehicles  includes  a  GPS  unit,  Doppler  Velocity  Log 
(DVL),  Acoustic  Doppler  Current  Profiler  (ADCP),  depth  sensor,  Inertial  Navigation 
System  (INS),  Inertial  Measurement  Unit  (IMU)  and  other  instruments  as  required  by  the 
user.  One  of  the  NPS  REMUS  100  AUVs  at  the  Center  for  Autonomous  Vehicle  Research 
is  shown  in  Figure  8.  This  AUV  is  installed  with  standard  REMUS  100  sensors  to  execute 
the  underwater  docking  mission.  The  color  segments  indicate  the  modules  used  to  make 
this  AUV. 


Figure  8.  One  of  the  NPS  REMUS  100  AUV 


When  the  vehicle  is  in  water,  it  can  use  acoustic  navigation  to  conduct  an 
undersea  survey  in  a  given  area.  When  the  AUV  gets  underway,  the  sensors  onboard  start 
sampling  and  recording  the  data  as  the  vehicle  proceeds  through  the  navigation 
objectives.  The  REMUS  100  has  a  control  computer  that  guides  the  vehicle’s  operation. 
REMUS  100  vehicles  could  replace  divers  in  dangerous  underwater  operations.  With  the 

battery  recharging  and  mission  data-transfer  capabilities  of  the  underwater  docking 
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stations,  the  REMUS  100  vehicles  could  spend  more  time  and  collect  more  meaningful 
data  underwater  at  lower  operating  costs.  Table  1  lists  the  physical  specifications  of  the 
NPS  REMUS  100  vehicle. 


Table  1.  NPS  REMUS  100  Vehicle  Specifications.  Source:  [24]. 


Platform 

REMUS  100 

Body  Type 

Torpedo 

Body  Size  (L  x  W  x  H) 

2.22  m  x  0.19  m  x  0.19  m 

Hull  Material 

Aluminum 

Weight 

52  kg  to  64.5  kg  in  air 

Operational  Depth 

3  m  to  100  m 

Typical  Search  Area 

1200  m  x  1000  m 

Dynamic  Buoyancy 

No 

Self-Righting 

Yes 

Obstacle  Avoidance 

No 

Endurance  (Nominal  Load) 

22  hrs  at  1.5  m/s  and  8  hrs  at  2.6  m/s 

Propulsion  System 

Thrusters  with  direct  drive  DC  brushless  motors 

Degrees  of  Freedom 

3  degrees  of  freedom  controlled  by  fins 

Nominal  Speed 

1.54  m/s 

Maximum  Forward  Speed 

2.6  m/s 

The  NPS  REMUS  100  AUV  is  equipped  with  the  built-in  sensors  and  the  modular 
sensors  listed  in  Table  2. 


Table  2.  NPS  REMUS  100  Onboard  Sensors 


Vehicle  built-in  sensors 

•  GPS 

•  Long  baseline  (LBL) 

•  Acoustic  Modem 

•  Kearfott  INS 

•  Side  Scan  Sonar 

•  Acoustic  Doppler  Current  Profiler  (ACDP)/Doppler 
Velocity  Log  (DVL) 

•  Conductivity  Temperature  Depth  sensor 

•  Optical  Backscatter 

Vehicle  modular  sensors 

•  Blueview  Forward  and  Downward  looking  sonar 

•  Downward  looking  Digital  Video  Recorder  (DVR) 

•  Digital  Ultra  Short  Baseline  (D-USBL) 
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Eric  B.  Bermudez  [24]  gives  a  full  description  of  the  NPS  REMUS  100  vehicle 
equipment.  The  key  component  is  the  USBL  hydrophone  on  the  bow  of  the  vehicle  and 
the  acoustic  modem  (located  underneath  just  forward  of  the  DVL).  The  D-USBL  is  an 
acoustic  navigational  aid  for  the  REMUS  vehicle,  according  to  the  REMUS/Swordfish 
Operations  &  Maintenance  Manual  [25].  The  D-USBL  consists  of  a  potted  assembly 
containing  a  four-channel  planar  hydrophone  array  with  integral  receiver/processor 
circuit  board.  The  REMUS  vehicle  uses  D-USBL  navigation  during  “transponder  home” 
objectives,  when  it  is  closer  to  the  transponder  baseline,  and  sometimes  when  only  one 
transponder  is  audible  [3]. 

According  to  [3],  during  the  D-USBL  navigation,  the  acoustic  modem  transducer 
sends  interrogation  pings  at  three-second  intervals.  The  D-USBL  array  receives  the 
acoustic  reply  from  any  standard  REMUS  transponder  in  the  field  of  view.  To  determine 
its  position  during  a  mission,  the  vehicle  calculates  the  range  and  bearing  to  the 
transponder  by  measuring  the  difference  in  receive-times  at  each  of  the  four  hydrophones 
[3],  Figure  9  shows  the  four-channel  planar  hydrophone  array. 


Digital  USBL  Array 


USBL 

Hydrophones 

“TOP" 


Figure  9.  D-USBL  Array  in  Its  Nose  Cap  with  Four-Channel  Planar 

Hydrophone  Array.  Source:  [3]. 


For  the  vehicle  to  receive  acoustic  replies  from  the  transponder  mounted  on  the 
docking  station,  the  transponder  needs  to  be  within  +/-35  degrees  of  the  D-USBL ’s 
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maximum  angle  of  expected  transponder  location.  Figure  10  illustrates  the  relative 
positions  of  the  vehicle  and  transponder  for  the  best  chances  of  receiving  acoustic  replies. 


Example  1) 

Good  D-USBL  Solution 


Transponder  within  t  17.5°  from 
Expected  bearing  of  - 10 *  from 

dead  a f read. 


'  175'  lutming  cone 
from  ‘expected*  bearing 


•  Transponder 

Transponder  Is  at  1 35'  Maximum  angle  of 
Expected  Transponder  location 
for  D-USBL  Navigation 


±  1 7i  6rten*»o  cone  m  A2mutft  and  Etewtion 

1 1  timing  angle  (torn ‘expected  beating  i 


Example  2) 

Good  D-USBL  Solution 


1 1?*  Mm  Itaerang  tn'je 


How  the  vehicle  calculates 
a  "GOOD"  D-USBL  Solution 


Example  3) 

■■Bad”  D-USBL  Solution 


Expected  Transponder 
bearing  -10‘  from  dead  ahead. 

r  1 7.5 'IWcmnQ  cone 
from  “ejipected"  bearing. 

@1  Transponder  actually  OUTSIDE  1 17. 5° 
from  Expected  bearing. 


Figure  10.  Cone  Angle  Specifications  for  D-USBL  Measurements.  Source:  [3]. 


Table  3  details  the  performance  specifications  of  the  D-USBL  from  the  REMUS/ 
Swordfish  Operations  &  Maintenance  Manual  [3]. 


Table  3.  D-USBL  Performance  Specifications.  Source:  [3]. 


Maximum  angle  from  dead  ahead 

+/-  35  degrees  in  Azimuth  and  in  elevation 

Listening  angle 

+/-  17.5  degrees  in  Azimuth  and  in  elevation 
from  the  expected  transponder  bearing 

Maximum  range 

2000  m 

Frequency  range 

20-30  kHz 

Bearing  accuracy 

+/-  2.4  degrees 

Bearing  resolution 

+/-  0.25  degrees 

Range  (travel  time)  accuracy 

+/-  1ms 

Range  (travel  time)  resolution 

+/-  80  micro-seconds 
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The  performance  specification  for  the  D-USBL  is  also  illustrated  in  Figure  1 1. 


'±  35'  Beamwidth  in  Azimuth  and  Elevation 
^  ^  (max  listening  angle) 

/ 

/ 


Figure  11.  D-USBL  Performance  Specifications.  Source:  [3]. 

B.  NPS  REMUS  100  DOCKING  STATION  DESCRIPTION 

The  purpose  of  the  underwater  docking  station  is  to  provide  a  battery  recharging 
facility  for  the  AUV  and  to  serve  as  an  interface  for  data  transfer  between  the  vehicle  and 
the  user  on  shore.  The  NPS  docking  station  was  designed  and  manufactured  by  WFIOI 
and  supplied  to  the  Center  for  Autonomous  Vehicle  Research  (CAVR).  The  docking 
station  came  with  two  ballast  tanks  that  help  lower  and  surface  the  docking  station.  The 
ballast  tanks  can  be  filled  using  a  diver’s  breathing  apparatus. 

The  NPS  docking  station  is  powered  by  a  9.6  kWh  battery  that  is  used  to  recharge 
the  AUV’s  batteries  approximately  10  times  and  to  supply  power  to  other  electronic 
components  of  the  docking  station.  The  NPS  docking  station  is  also  equipped  with  a 
USBL  transponder,  which  is  used  for  terminal  homing  of  the  AUV.  The  docking  station 
has  a  linear  actuator  with  guide  pins  that  help  align  the  AUV  after  a  successful  docking 
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operation  to  successfully  recharge  the  batteries  and  connect  to  the  Ethernet  cable  for  data 
transfer  between  the  dock  and  the  vehicle. 

The  entrance  to  the  docking  station  has  a  square-shaped  opening  of  0.7  m  by  0.7 
m  at  the  larger  end  of  the  funnel-shaped  entrance.  The  smaller  end  of  the  funnel  is 
circular  with  a  diameter  of  0.22  m  and  becomes  the  entrance  for  the  cylindrical  portion  of 
the  AUV  when  it  is  successfully  docked.  The  length  of  the  cylindrical  opening  is  0.93  m. 

The  D-USBL  transponder  is  mounted  0.42  m  above  and  0.15  m  to  the  left  of  the 
cylindrical  centerline,  0.56  m  behind  the  square-shaped  end  of  the  funnel.  The  NPS 
docking  station  is  also  mounted  with  an  underwater  camera  that  helps  the  user  to  view 
and  make  necessary  adjustments  to  the  docking  operation.  The  camera  is  cable-connected 
to  the  dock,  which  can  be  connected  to  a  gateway  buoy  for  wireless  communication  with 
users  at  the  surface.  Figure  12  shows  the  WHOI  docking  station  which  is  utilized  by  the 
NPS  CAVR  in  the  vicinity  of  the  Monterey  Inner  Shelf  Observatory  in  Monterey  Bay, 
California  for  undersea  docking  experiments. 


Docking  entrance 

OJmx  0.7m  JMLbansponder 
Data 

Transmission  cables 

Battery  Recharging 
em 


NPS  assisting 
diver. 


^ _ Manual  Buoyancy 

system 


Plexiglass  for 
shielding 


Figure  12.  WHOI  Docking  Station  used  by  NPS  CAVR  in  Monterey  Bay, 

California.  Source:  [1]. 
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III.  PROBLEM  DESCRIPTION  AND  ANALYSIS 


The  purpose  of  this  study  is  to  develop  a  methodology  for  position  estimation  by 
which  sparse,  erroneous,  and  inconsistent  sensor  observations  can  be  used  for  AUV 
tenninal  homing  to  an  undersea  docking  station.  An  undersea  docking  station  offers  great 
potential  for  increasing  at-sea  time  and  reducing  survey  costs  of  AUVs.  A  key  part  of  this 
system  is  the  AUV’s  ability  to  successfully  dock.  This  is  a  terminal  homing  problem 
where  a  robust  solution  addresses  Guidance,  Control  and  Navigation  (GNC).  Guidance  is 
defined  as  the  process  of  directing  the  AUV  to  a  desired  path  or  position.  Control  is 
defined  as  the  process  of  maintaining  the  desired  path  or  position  using  the  feedback 
controller  and  considering  the  external  factors.  Navigation  is  defined  as  the  process  of 
localizing  the  vehicle’s  position  by  using  position  estimation  methods.  Part  of  the  process 
for  successful  docking  is  estimating  the  AUV’s  position.  The  AUV’s  D-USBL  sensor 
system  provides  range  and  bearing  measurements  to  the  docking  station.  The  D-USBL 
range  and  bearing  measurements  are  nonlinear,  sparse,  erroneous  and  inconsistent. 

A.  PROBLEM  DESCRIPTION 

For  the  purposes  of  this  thesis,  the  REMUS  AUV  will  use  a  north-east-down 
(NED)  coordinate  system.  Using  a  local  reference  frame,  we  define  the  following 
variables:  longitudinal  position  (x),  lateral  position  (y),  downward  position  (z),  surge 
velocity  (u),  sway  velocity  (v),  heave  velocity  (w),  roll  angle  (O),  pitch  angle  (0),  yaw 
angle  (vj. /),  as  described  in  T.  I.  Fossen  [25],  These  parameters  are  shown  in  Figure  13  at 
their  corresponding  axis  of  the  AUV.  The  Inertial  reference  frame  is  illustrated  by  the 
capital  letters  X,  Y  and  Z  in  North,  East  and  Down  directions,  respectively.  The  down 
direction  is  illustrated  by  the  broken  lines  in  both  Inertial  and  Local  reference  frames. 
The  Local  reference  frame  is  illustrated  by  the  small  letter  x,  y  and  z  directions  which  are 
on  the  body  of  the  vehicle.  The  vehicle  will  also  be  controlled  by  the  yaw-rate  (r). 
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North,  X 


Figure  13.  REMUS  100  AUV  Axis  and  Control  Parameters.  Source:  [25]. 

In  this  study,  the  simulated  vehicle  is  an  AUV  following  a  set  of  waypoints  and 
controlled  by  a  cross-track-error  controller.  The  AUV  is  released  at  a  starting  position 
closer  to  the  first  waypoint.  The  cross-track-error  controller  always  attempts  to  keep  the 
AUV  on  its  intended  track  although  the  dead  reckoning  instruments  cause  it  to  deviate 
from  the  track  as  navigational  errors  increase  with  distance  traveled. 

Figure  14  depicts  the  underwater  setup  as  explained  in  this  section.  The  vehicle  is 
shown  just  past  the  first  waypoint  and  approaching  the  second.  The  AUV  track  is  shown 
as  a  red  broken  line  while  the  intended  track  is  shown  as  a  black  broken  line. 
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Figure  14.  AUV  Docking  Simulation  Setup  from  Surface  to 
Underwater  Docking  Station 


As  the  AUV  follows  the  waypoints,  the  AUV  can  receive  measurements  at  up  to 
300m  away  from  the  USBL  transponder  mounted  on  the  docking  station.  The  USBL 
measurements  received  by  the  AUV  are  arbitrarily  distributed  and  typically  non- 
Gaussian.  This  makes  it  difficult  to  choose  a  position  estimating  technique  that  produces 
an  accurate  position  estimate  for  the  AUV.  Wan  and  van  der  Merve  [26]  describes  how 
the  EKF  assumes  a  Gaussian  random  variable  (GRV)  distributed  state  with  a  mean  and 
variance  and  performs  position  estimation,  which  is  accurate  up  to  first-order  Taylor 
series  expansion.  Given  that  the  EKF  can  produce  huge  errors  on  highly  nonlinear 
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systems,  the  UKF  is  also  explored  in  this  thesis.  The  UKF  also  assumes  the  state  is  a 
GRV  distribution,  but  the  UKF  uses  carefully  chosen  sigma  points  to  represent  the  actual 
state  distribution  and  produces  an  error  of  at  least  second-order  accuracy  [26].  This 
tenninal  homing  problem  is  nonlinear  due  to  the  fact  that  the  state  propagation  equations 
(the  system  model)  and  the  range  and  bearing  measurements  (the  measurement  model) 
are  both  nonlinear. 

Figure  15  shows  how  the  EKF  and  UKF  represent  the  state  distribution.  In  this 
figure,  the  actual  state  is  shown  on  the  left  with  mean  and  covariance.  The  linearized 
EKF  is  shown  in  the  middle  with  Gaussian  approximation  of  the  state  while  the 
unscented  transform  (UT),  a  UKF  component,  is  shown  on  the  right  with  five  carefully 
placed  sigma  points  to  represent  the  actual  state  distribution.  The  true  mean,  as  indicated 
by  a  small  black  circle,  is  shown  at  the  bottom  of  each  of  these  representations.  As 
expected,  the  UKF  locates  its  mean  very  close  to  the  actual  mean  with  a  small  variance. 
Meanwhile,  the  EKF  mean  and  covariance  (shown  in  red)  differ  noticeably  from  the 
actual  mean  and  covariance  (shown  in  black). 
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Figure  15.  State  Distribution  as  Represented  by  EKF  and  UKF  (UT). 

Source:  [27]. 


To  further  describe  the  challenge  of  selecting  the  best  position  estimation 
technique,  Figure  16  depicts  a  high-level  view  of  the  AUV  docking  situation  and  the  way 
the  EKF  and  UKF  represent  the  measurements.  As  shown,  the  UKF  distribution  can 
account  for  many  USBL  measurements  due  to  the  flexibility  of  placing  the  sigma  points. 
The  EKF  is  limited  to  the  GRV  distribution  assumption  and  can  only  accommodate  few 
measurements  in  its  distribution.  This  causes  the  EKF  to  be  less  accurate  on  highly 
nonlinear  measurements.  In  this  figure,  the  AUV  is  shown  approaching  the  region  with  a 
high  concentration  of  USBL  measurements.  The  AUV  cannot  use  all  measurements  due 
to  its  dynamic  limitations.  The  position  estimation  technique  should,  therefore,  produce 
an  optimal  mean  for  the  AUV  to  move  in  the  right  direction. 
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Figure  16.  REMUS  100  AUV  Docking  Scenario  and  State  Representation  by 

UKF  and  EKF 

A  general  filtering  approach  is  summarized  in  the  flow  chart  provided  in  Figure 
17.  The  flow  chart  shows  the  measurement  model  on  the  left  with  the  measured  state.  The 
measurements  produced  by  the  built-in  AUV  sensors  such  as  GPS,  INS  and  DVL  are 
integrated  into  a  single  source  of  measurements  with  an  update  rate  of  not  more  than  10 
Hz.  These  measurements  are  then  combined  with  the  sparse,  inconsistent  and  erroneous 
D-USBL  measurements  with  update  rate  of  about  0.2Hz  to  1Hz.  These  measurements  are 
combined  in  a  data  fusion  process,  which  comprises  the  position  estimation  methods 
listed.  The  system  model  also  provides  the  estimated  state  to  the  data  fusion  block,  which 
fuses  all  measurements  using  the  EKF  or  UKF  with  Kalman  gain  or  by  using  the  MHE 
with  epi-splines.  The  data  fusion  results  in  the  optimal  estimated  state,  as  illustrated  on 
the  right-end  of  the  flow  diagram.  The  optimal  estimated  state  is  then  fed  back  to  the 
system  model  as  a  new  predicted  state  of  the  AUV  in  the  next  time  step.  If  D-USBL 
measurements  are  not  available,  the  vehicle  uses  the  integrated  system  measurements  to 
dead  reckon  to  its  future  position. 
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Figure  17.  General  Data  Filtering/Fusion  Flow  Chart 


B.  DESCRIPTION  OF  OPTIMAL  ESTIMATION  USING  EKF 

Rhudy  and  Gu  [28]  describe  the  EKF  as  an  important  technique  in  optimally 
estimating  the  state  of  engineering  systems  using  an  optimal  observer  gain  to  minimize 
the  expected  mean  squared  error  of  the  estimate.  The  EKF  assumes  the  linear  Gaussian 
nature  of  the  posterior  state  with  mean  and  covariance  to  obtain  an  optimal  solution.  The 
EKF  approaches  the  nonlinearity  of  the  system  by  applying  the  analytical  linearization, 
which  involves  using  Jacobian  matrices.  The  calculation  of  the  Jacobian  is  a 
straightforward  operation,  but  it  has  the  potential  for  errors. 

The  EKF  is  more  effective  in  nonlinear  systems  up  to  the  second  degree  of 
nonlinearity.  The  EKF  becomes  less  efficient  as  the  degree  of  nonlinearity  increases. 
Even  though  the  EKF  is  unlikely  to  be  used  on  systems  with  higher  levels  of  nonlinearity, 
it  is  simpler  to  apply  and  understand  than  the  filters  used  for  highly  nonlinear  systems. 

The  main  drawback  for  the  EKF  is  that  because  solutions  are  restricted  by  the 
Gaussian  assumption,  highly  nonlinear  effects  of  the  system  are  not  included  in  the 


29 


variance  estimate.  This  causes  the  EKF  to  perfonn  poorly  on  highly  nonlinear  systems. 
According  to  Einicke  and  White  [29],  the  EKF  linearizes  the  nonlinear  measurement 
model  for  its  current  state  estimate  and  uses  the  Kalman  filter  to  propagate  the  state 
forward  in  time. 

The  EKF  uses  five  equations  to  obtain  the  optimal  solution  of  the  state  estimation. 
The  equations  are  as  follows: 

Prediction  Stage: 

System  Model 


xk\k-i  =  /(4- 1,  Ufc-i)  +Wk  wk  =  N (0,  a) 

(1) 

Measurement  Model 

Zk  =  h(x/c_1)  +  vk  vk=  iV( 0,  cr) 

(2) 

Prior/Prediction  Covariance  Matrix 

Pk\k~l  ~  Ak-iPk-l\k-lAk-i  +  Qk- 1 

(3) 

Kalman  Gain: 

Kk  =  Pkik-iHliHbP^Hl  + 

(4) 

Correction  Stage: 

xk\k  —  xk\k-l  +  Kk(^k  ^k^-klk-l} 

(5) 

Pk\k  —  Pk\k-iO  —  KkHk) 

(6) 

In  (1)  xfc|fc_1  is  the  new  estimation  at  time  k  based  on  prediction  obtained  from 
time  k-1 ,  and  ufc_ i  is  the  input  vector  at  time  k-1  and  is  ignored  for  this  study.  Function/ 
is  a  nonlinear  continuous  function;  therefore,  it  is  differentiable  such  that  Ak is  a 
Jacobian  based  on  time  k-1  obtained  by  differentiating  the  function  /  with  respect  to  the 
state  xkik_1  as  given  by 

df 

Ak-i  =-^r—\xk-1  (7) 

axk~i 

In  (2)  Zk  is  the  measurement  model  based  on  time  k,  h  is  the  nonlinear  function 
relating  the  current  process  state  xk  to  the  current  measurement  Zk,  and  vk  is  the 

measurement  noise.  In  (3)  Pk\k- 1  is  the  covariance  of  the  predicted  state  at  time  k  based 

30 


on  the  prediction  from  time  k-1,  and  Q/c-i  is  the  covariance  of  the  linearized  process 
model. 


In  (4)  Kk  is  the  Kalman  filter  gain  at  time  k,  while  Hk  is  the  Jacobian  obtained  by 
linearizing  the  measurement  function  h  around  the  current  state  as  given  by 


dh 

Ht  ~ 

and  Rk  is  the  covariance  of  the  linearized  measurement  model. 


(8) 


In  (5)  xk\k  is  the  estimate  of  the  mean  at  time  k  based  on  new  measurements  at 
time  k  and  x/c|/c_1,  while  Pk\k  in  (6)  is  the  covariance  of  the  current  estimate  xk\k. 


In  this  study,  the  state  transition  matrix  (A)  is  used  to  relate  the  state  estimate  to 
the  new  prediction.  The  equation  showing  the  relationship  between  the  state  estimate  and 
the  current  prediction  is  given  by  the  commonly  used  and  convenient  first-order  linear 
differential  equation: 


x  —  Ax  (9) 

the  solution  of  which  is  given  by 

0  =  eAdt  (10) 


where  dt  is  the  time  increment  in  seconds.  With  this  relationship,  (1)  and  (2)  become 


Xk\k-1  OkXk-i  (11) 

Pk\k~l  ~  0/A-10fc  +  Qk-l  (12) 

The  equations  of  motion  of  the  vehicle  underwater  in  NED  coordinates,  assuming 


roll  angle  is  zero,  are  as  follows: 


xk  —  xk_t  +  ucos(d)  cos(xp )  dt  —  vsin(xp)dt 
yk  =  Yk-i  +  ucos(Q )  sin(xp )  dt  +  vcos(xp)dt 
zk  —  zk_1  +  usin(6)dt 


The  state  is  then  given  by 
dxk 


dt 


=  xk  =  ucos(9 )  cos(xp )  —  vsin(xp ) 


dyk 

dt 


—  yk  —  ucos(9 )  sin(xp )  +  vcos(xp ) 


(13) 

(14) 

(15) 


(16) 

(17) 
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dzk 

—  =  zk=  usin(d) 


(18) 


Therefore,  the  state  transition  matrix  A  is  given  by 
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And  the  matrix  x  is  given  by 

x 

y 

z 
u 
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e 
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Range 
Bearing 

C.  DESCRIPTION  OF  OPTIMAL  ESTIMATION  USING  UKF 

The  UKF  is  one  of  the  most  popular  state  estimation  techniques  used  for 
nonlinear  systems.  The  UKF  is  from  the  same  family  of  filtering  systems  as  the  EKF.  The 
UKF  is  an  improved  method  of  filtering  from  the  EKF.  Since  the  EKF  is  known  for 
producing  large  errors  in  higher  order  systems,  it  is  considered  a  sub-optimal  filter.  The 
UKF,  on  the  other  hand,  is  considered  an  optimal  filter. 

The  UKF  employs  the  UT,  which  is  used  in  calculating  the  statistics  of  a  random 
variable  undergoing  a  nonlinear  transformation  [26].  The  UKF  is  accurate  up  to  third- 
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order  Gaussian  inputs  for  all  nonlinearities  and  at  least  second-order  for  non-Gaussian 
inputs  [26]. 

The  UKF  is  an  extension  of  the  UT.  The  UKF  is  easier  to  apply  than  the  EKF 
because  it  does  not  make  any  use  of  the  Jacobians,  which  may  be  challenging  to  evaluate 
and  result  in  approximation  errors  [26].  The  UKF  employs  deterministically-chosen 
weighted  sample  points  known  as  sigma  points  to  compute  its  version  of  the  Jacobian  and 
Hessian  matrices  without  perfonning  analytical  differentiation.  These  sigma  points  are 
chosen  to  completely  capture  the  true  mean  and  covariance  of  current  state  variables.  The 
UKF  is  accurate  up  to  third-order  Taylor  series  for  any  nonlinearity  unless  the  prior 
random  variable  has  a  symmetric  distribution  such  as  an  exponential  family  of  probability 
density  functions  [26]. 


The  following  is  the  final  algorithm  used  by  the  UKF  to  provide  an  accurate  state 
estimate  and  its  covariance.  The  UKF  uses  sigma  points  calculated  as  follows: 


A  —  a2  (L  +  k)  —  L 


(19) 


where  A  is  the  scaling  parameter,  and  a  is  the  spread  of  the  sigma  points.  The  parameter 
a  ranges  between  1CU4  and  1.  When  a  is  small,  the  sigma  points  get  closer  to  each  other, 
and  when  a  is  big,  the  sigma  points  scatter.  The  parameter  L  is  the  length  of  the  state 
vector,  while  k  is  a  tertiary  scaling  parameter,  which  is  equal  to  zero  in  most  cases.  This 
scaling  parameter  is  used  as  a  tuning  “knob”  to  reduce  estimation  errors,  as  shown  by 
Julier  and  Uhlmann  [30].  The  weighted  vector  for  the  mean  (eta  with  superscript  m  for 
mean  and  subscript  0  for  initial  condition)  is  given  by 


V™ 


A 

L  +  A 


(20) 


The  weighted  vector  for  covariance  (eta  with  superscript  c  for  covariance  and 
subscript  0  for  initial  condition)  is  given  by 

Vo  —  +  1  —  a2  +  (3  (21) 


In  (21)  (3  is  known  as  the  secondary  scaling  parameter  with  an  optimal  value  of  2  for 
Gaussian  distributions. 
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1 


riT  =  VCi 


2  (L+A) 


/ori  =  1, ...  2 L  columns 


(22) 

(23) 

(24) 


rj  =  Vo  +  Vi 
Vc  =  Vo+Vi 

The  number  of  sigma  points  (n)  depends  on  the  length  of  the  state  vector,  and  the 
relationship  is  given  by 

n  =  2L  +  1  (25) 

The  size  of  the  sigma  point  matrix  is  given  by 

Xk- 1  =  [4-i4-i  +  VL  +  A7Pfe_!  xfc_1-VL  +  (26) 

In  (26)  xk_,  is  the  prior  state  estimate  and  Pk-i  is  its  covariance.  ^Pk-i  is  given  by  the 
Cholesky  method  CholiT/^  J,  where  Pk-i  is  the  lower  triangular  matrix  and  k  is  the  time 
of  the  computation.  The  sigma  points  are  propagated  through  prediction  using 

Xlk\k-i  =  /fe-i.Wfc-i)  (27) 

In  (27)  i  —  0,1, ...  2 L  columns  of  the  matrix,  and  /  is  the  nonlinear  function  describing 
the  dynamics  of  the  system.  uk_1  is  the  vector  containing  the  input  parameters  of  the 
system. 

The  mean  is  then  predicted  by 


2  L 


4|fe-r  =^VilXik\k-i 


(28) 


1=0 


In  (28)  vT1  s  the  weighted  vector  for  the  mean. 

The  covariance  of  the  predicted  state  is  then  given  by 

2  L 

Pk\k-i  =  Qk-i  +  ^  vci  (xlk\k-i  ~  %k\k-i)(xlk\k-i  -  4|/c-i)7 


(29) 


1=0 


In  (29)  Q/c-i  is  the  process  error  covariance  matrix  and  rjf  is  the  weighted  vector  for  the 
covariance. 

The  observation  transfonnation  is  computed  by  propagating  each  sigma  point 
through  the  observation  as 

Vk\k-i  =  h(Xk\k-vuk)  (30) 
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where  i  —  0,1,  ■■■  2 L,  and  h  is  a  nonlinear  observation  function  consisting  of  range  and 
bearing  nonlinear  functions  used  in  many  underwater  vehicle  tracking  algorithms.  The 
mean  predicted  output  is 

2  L 

yk\k-i  =  'YJiiilVk\k-i  (31) 

i= 0 

where  ryffi s  the  weighted  vector  for  the  mean,  and  U’kik-i  is  the  propagation  of  sigma 
points. 

The  covariance  of  the  predicted  output  is  given  by 

2  L 

Pky  =  Rk  +  ^ Vi  M|fc-1  - 9k\k-i) W|fc-1  - 9k\k-iY  (32) 

i= o 

In  (32)  Rk  is  the  measurement  covariance.  The  cross-covariance  of  the  state  and  the 
output  is  given  by 

2  L 

PkV  =  X  0Ck\k-i  ~  xk\k-i) W|fc-i  -  9k\k-iY  (33) 

i= 0 

The  measurement  update  stage  of  the  UKF  is  similar  to  that  of  the  Kalman  filter 
and  the  EKF.  The  measurement  update  stage  is  given  by 

k„ = ifwy1  (34) 

In  (34)  Kk  is  the  Kalman  gain,  and  by 

4  —  4|fc-l  +  Kk{%k  -  9k\k-l)  (35) 

In  (35)  xk  is  the  state  estimate  update.  The  error  covariance  update  is  then  given  by 

Pk  =  Pk\k-i  ~  KkPkVKk  (36) 

D.  SIMULATED  ANALYSIS  OL  THE  UKL  AND  EKE  POSITION¬ 
ESTIMATING  TECHNIQUES 

A  simulation  was  conducted  using  the  previous  USBL  measurements  for  the 
REMUS  100  AUV  docking  station.  Dead  reckoning  measurements  were  simulated  using 
the  cross-track-error  control  and  dead  reckoning  equations.  A  total  of  ten  runs  were  used 
to  evaluate  the  ability  of  the  UKF  and  EKF  in  estimating  the  position  of  the  vehicle.  A 
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small  bias  associated  with  the  heading  was  assumed  and  used  for  every  run.  The  initial 
uncertainty  of  the  vehicle  with  respect  to  the  simulated  USBL  measurements  was  also 
assumed  and  used. 

The  docking  error  was  regarded  as  the  distance  from  the  center  of  the  entrance  of 
the  docking  station  to  the  longitudinal  centerline  of  the  AUV.  Figure  18  illustrates  how 
the  error  was  measured.  In  Figure  18  the  vehicle  is  on  the  surface  and  the  docking  station 
is  shown  as  a  white  rectangular  frame.  The  reader  should  note  that  this  is  an  illustration 
for  docking  error  measurement  only.  For  an  AUY  to  dock,  the  error  (e)  should  be  less 
than  0.35m.  The  size  of  the  square  entrance  is  0.7m  in  height  and  0.7m  in  breath,  and 
0.35m  is  the  radius  of  the  entrance. 


Figure  18.  Docking  Error  Computation  Illustration 


Table  4  shows  the  results  of  the  simulation.  No  docking  success  was  achieved  in 
any  run;  however,  as  expected,  the  UKF  managed  to  position  the  vehicle  much  closer  to 
the  docking  station  than  the  EKF  technique. 
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Table  4.  Simulated  UKF  and  EKF  Undersea  Docking  Results 


Runs 

USBL 

Update  rate 
in  (Hz) 

Bias 

in 

(%  distance 
travelled) 

Initial 

Uncertainty 
in  (m) 

UKF 

Docking  error 
(m) 

EKF 

Docking  error 
(m) 

Docking 

Success 

REMUS 

UKF 

EKF 

1 

0.401 

0.70% 

10.8 

4.323 

4.331 

No 

No 

No 

2 

0.299 

1.00% 

8.5 

8.109 

8.129 

No 

No 

No 

3 

0.319 

0.90% 

11.4 

2.908 

2.911 

No 

No 

No 

4 

0.428 

0.60% 

7.5 

4.092 

4.102 

No 

No 

No 

5 

0.436 

0.70% 

6.7 

3.913 

3.924 

No 

No 

No 

6 

0.252 

0.60% 

4.2 

2.690 

2.698 

No 

No 

No 

7 

0.209 

0.40% 

3.0 

5.629 

5.651 

No 

No 

No 

8 

0.351 

0.60% 

3.0 

3.008 

3.018 

No 

No 

No 

9 

0.255 

0.40% 

3.3 

2.916 

2.925 

No 

No 

No 

10 

0.343 

0.50% 

3.0 

6.780 

6.801 

No 

No 

No 

Average 

0.329 

0.64% 

6.1 

4.437 

4.449 

Run  5  had  a  higher  update  rate;  but  the  measurements  were  distributed  poorly 
such  that  no  docking  success  was  possible  using  the  UKF  and  EKF.  Figure  19  shows  the 
positions  of  the  vehicle  as  represented  by  the  curves  associated  with  each  estimating 
method.  As  expected,  the  UKF  managed  to  position  the  vehicle  a  little  closer  to  the 
docking  station  than  the  EKF. 
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Figure  19.  Run  5  with  High  Frequency  Update  Rate  and  Poor  Docking  Results 


Run  7  had  the  lowest  frequency  update  of  USBL  measurements.  The  results 
obtained  from  this  run  were  as  expected,  owing  to  the  low  USBL  sensor  update  rate. 
Figure  20  provides  an  illustration  of  the  measurements’  locations.  As  depicted,  docking 
was  impossible  when  the  measurements  were  far  from  the  intended  track. 
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Figure  20.  Run  7  with  Lowest  Update  Rate  and  Poor  Docking  Results 
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IV.  MOVING  HORIZON  ESTIMATION  WITH  EPI-SPLINES 


A.  INTRODUCTION 

In  the  last  chapter  the  EKF  and  UKF  were  used  for  AUV  position  estimation.  The 
performance  was  poor  due  to  the  sparse,  erroneous,  sporadic  USBL  measurements.  In 
this  chapter  the  Moving  Horizon  Estimation  is  presented  as  an  alternative.  It  is  the  state 
estimation  method  that  uses  ideally  all  available  observed  system  dynamics 
measurements  as  constraints  to  estimate  the  state  of  a  nonlinear  or  constrained  system 
through  solving  an  optimization  problem  [21].  The  strength  of  the  approach  is  that  it 
handles  the  constraints  in  a  more  systematic  way.  Furthennore  a  new,  modified  MHE  has 
been  developed.  It  leverages  the  fact  that  over  relatively  short  time  horizons  the  AUV 
trajectory  has  limited  positional  uncertainty.  This  can  be  combined  with  the  USBL 
measurements  to  provide  a  more  accurate,  reliable  position  solution.  The  technique 
utilizes  an  epi-spline  technique  for  non-linear  least  squares  estimation  of  the  moving 
horizon  estimation.  Epi-splines  are  “the  newly  developed  piecewise  polynomial  functions 
described  by  the  finite  number  of  parameters,  which  are  exceptionally  flexible.”  Royset 
and  Wets  [17].  They  are  useful  for  their  ability  to  incorporate  both  soft  and  hard 
constraints  Royset  and  Wets  [31]. 

The  EKF  and  UKF  aimed  to  solve  (37),  the  maximum  likelihood  estimate  for  the 
systems  of  equations  (1)  and  (2). 

*k  =  ar§ max p(xk \z0,  ...,zk)  (37) 

where  p(xk\z0, ... ,zk )  is  the  probability  that  the  system  state  is  xk  given  measurements 
z0,...,zk.  As  shown  in  Haseltime  and  Rawlings  [32],  the  MHE  maximizes  the  joint 
probability  for  the  trajectory  of  the  state  values  such  that 

xk , ... , Xj.  —  arg  max  p(xk,  ...,xT \z0,  ...,zk)  (38) 

According  to  [32],  for  constrained  nonlinear  systems,  logarithmic  transformation 
of  (37)  is  computationally  easy: 

arg  max  -\ogp(xk, ... ,xT\z0, ... ,zk)  =  arg  max  p(xk, ... ,xT\z0, ... ,zk)  (39) 

X^,...,XT  X^^.^Xj  v  ' 
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because,  by  assuming  that  the  a  priori  state  estimate  is  a  N (x0,  £0)  distributed  noise, 
minimization  of  (39),  which  is  subject  to  the  nonlinear  model  (1),  gives  rise  to  a  least- 
squares  optimization: 


r-i 


0T  =  min  f\x0)  +  >  wlQ^Wk  +  > 

xk,...,x  T  Z_i  Z_i 

k= 0  k= 0 


vlRk'vk 


(40) 


Such  that:  /Xx0)  =  (x0  —  x0)riT1(x0  —  x0)  (41) 

xk\k-i  =  ^(4-i.Ufc-i)  +  wk  (42) 

Zk  =  hCxfc.i)  +  vk  (43) 

x  eX,  w  eW ,  v  eV  (44) 


Equations  (40)  through  (43)  are  collectively  called  the  full  information  problem, 
and  X,  W  and  V  contain  the  values  of  the  system  state,  system  disturbance,  and 
measurement  disturbance,  respectively  [32],  The  X  component  contains  values  for  the 
vehicle  constraints  such  as  turn  rate,  pitch  rate,  and  velocity  [32].  This  problem  is 
difficult  to  solve  due  to  its  computational  complexity.  For  this  reason,  the  nonlinear  least 
squares  optimization  technique  is  applied  to  solve  the  full  information  problem. 

In  order  to  use  the  MHE  approach  for  real-time  applications,  the  optimization 
process  should  be  quick  and  accurate;  therefore,  the  epi-splines  were  used  for  the 
optimization  process.  Royset  and  Wets  [33]  derived  the  equations  used  for  the  epi-splines 
MATLAB  programming  code  in  this  thesis.  The  equations  used  in  this  thesis  were 
extracted  from  Horner  and  Mqana  [1],  Royset  and  Wets  [17],  [31]  and  [33],  and  Peter 
Tydingco  [34],  and  they  appear  in  the  following  paragraphs.  The  epi-splines  are  defined 
as  the  piecewise  polynomials  with  extended,  real  valued,  lower  semi-continuous 
functions  on  Rn  and  defined  by  the  polynomial  order,  the  number  of  partitions  (S)  and  the 
mesh  m  —  {mk}k=0  [1],  See  Homer  and  Mqana  [1]  for  an  in-depth  analysis  of  the  epi- 
splines. 

During  the  optimization  process,  the  epi-splines  produce  the  vehicle’s  trajectory 
from  the  vehicle  state.  The  next  step  of  the  optimization  process  fits  the  vehicle  trajectory 
to  the  USBL  measurements  accordingly  [1].  The  first  step  of  the  optimization  process 
with  epi-splines  is  described  as  follows: 
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(45) 


The  epi-splines  are  piecewise  polynomials  at  every  segment  with 
/(d)  =  [dp1  +  a^di  +  a^df]  representing  the  polynomial  for  vehicle  data  and 
Aj  =  |/(dt)  —  /(d)  |  representing  the  absolute  value  of  the  difference  between  the  vehicle 
data  and  the  predicted  vehicle  data.  The  constraints  enumerated  in  (45)  are  used  to 
minimize  the  average  error  between  the  vehicle  data  points  and  the  fitted  curve.  The  first 
and  second  constraints  in  (45)  are  for  minimizing  the  error  associated  with  the  objective 
function.  The  third  constraint  is  for  ensuring  continuity  of  the  function.  The  fourth 
constraint  is  for  ensuring  continuity  of  the  derivative  of  the  function.  The  fifth  constraint 
sets  the  upper  and  lower  boundaries  for  the  absolute  difference  between  the  vehicle  data 
and  the  predicted  vehicle  data.  The  second  step  of  the  optimization  minimizes  the  error 
between  the  USBL  measurements  and  the  curve  fitted  by  the  epi-spline  optimization 
process.  The  resulting  output  is  the  MHE  trajectory  that  represents  the  optimal  estimate 
of  the  path  of  the  vehicle. 


B.  MOVING  HORIZON  ESTIMATION  WITH  EPI-SPLINE  SIMULATION 

The  MHE  with  epi-splines  was  applied  to  past  docking  station  runs  of  the  real 
REMUS  100  AUV.  The  AUV’s  dead  reckoning  measurements  were  used  as  constraints 
for  the  epi-splines  to  produce  the  trajectory  of  the  AUV.  The  nonlinear  least  squares 
method  was  then  used  to  fit  the  trajectory  of  the  AUV  through  the  available  USBL 
measurements  in  accordance  with  the  covariance  of  the  dead  reckoning  system  and 
USBL  measurements. 
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The  MHE  begins  after  the  window  has  the  first  five  USBL  measurements.  Figure 
21  illustrates  how  the  MHE  with  epi-splines  works.  The  MHE  with  epi-splines  uses  the 
measurements  in  the  window  and  produces  a  curve  for  the  estimated  states  associated 
with  the  measurements  in  the  window.  The  MHE  is  applied  only  when  an  additional 
measurement  has  been  obtained.  The  window  is  fixed  to  the  first  USBL  measurement  and 
extends  every  time  there  is  a  new  USBL  measurement.  The  process  repeats  until  the  epi- 
splines  are  applied  in  the  window  containing  all  USBL  measurements. 


Every  time  the  epi-splines  produce  the  estimated  means  curve,  the  curve  rotates 
using  the  covariance  associated  with  the  USBL  measurements  and  the  covariance 
associated  with  the  dead  reckoning  measurements. 
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Figure  22  shows  how  the  epi-splines  with  nonlinear  least  squares  fit  the  curve  for 
the  means  of  the  available  USBL  measurements  and  in  accordance  with  the  dynamics  of 
the  vehicle  in  the  current  window. 


Figure  22.  Illustration  of  How  the  Optimal  Curve  Is  Fitted 


Notably,  the  thick  blue  curve  has  the  same  trajectory  as  the  curve  for  a  dead 
reckoning  AUV  shown  in  Figure  22.  While  the  covariance  associated  with  dead 
reckoning  measurements  grows  linearly  over  distance  traveled,  the  covariance  associated 
with  the  USBL  measurements  drops  linearly  as  they  get  closer  to  the  docking  station. 

For  this  analysis,  the  vehicle  started  at  a  position  just  before  it  could  receive  the 
USBL  measurements.  During  the  complete  run  of  the  REMUS  100  AUV,  the  vehicle 
starts  closer  to  the  docking  station  and  travels  about  150  m  dead  reckoning  before 
receiving  USBL  measurements.  When  the  vehicle  approaches  the  zone  where  it  can 
receive  USBL  measurements,  the  accumulated  dead  reckoning  error  is  high.  This 
accumulated  error  is  used  in  this  thesis  as  the  initial  vehicle  uncertainty.  Sometimes  the 
vehicle  moves  closer  to  the  zone  where  the  chances  of  receiving  USBL  measurements  are 
high,  making  the  initial  uncertainty  of  the  vehicle  small. 
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The  initial  uncertainty  is  also  used  as  the  variance  associated  with  dead  reckoning 
measurements  when  rotating  the  epi-splines  results  according  to  the  covariance 
associated  with  USBL  and  dead  reckoning  measurements.  Figure  23  illustrates  the 
rotation  of  the  curves  produced  by  the  epi-splines  for  every  window.  The  curves  start  by 
settling  in  between  the  dead  reckoning  and  USBL  measurements,  but  as  the  window 
stretches,  the  curves  move  away  from  the  dead  reckoning  measurements  and  closer  to  the 
USBL  measurements. 
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Figure  23.  MATLAB  Results  Showing  the  Epi-Spline  Optimal  Solution  in  Every 

Window 

Figure  24  further  illustrates  the  rotation  of  the  curve  for  the  estimated  means  and 
its  deviation  from  the  dead  reckoning  curve  as  the  vehicle  approaches  the  docking  station. 
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C.  MOVING  HORIZON  ESTIMATION  WITH  EPI-SPLINE  ALGORITHM 

The  algorithm  used  for  the  MHE  is  illustrated  in  Table  5.  The  inputs  to  the  epi- 
splines  algorithm  are  the  MHE  window  size  ( MHEWS )  ,  the  system  state  for  the  window 
size  (%T),  the  covariance  for  the  system  state  of  the  window  size  (PT),  the  constraints 
associated  with  vehicle  dynamics  as  given  by  the  measurement  vector  ( Zk ),  and  the 
USBL  measurements  in  the  window  size  (ZUSBL). 

For  accurate  approximation  results,  the  MHE  horizon  should  contain  all  the 
measurements;  however,  this  requires  significant  computational  memory  from  the  system 
[21].  The  window  size  used  for  this  thesis  started  with  five  USBL  measurements,  and  it 
kept  stretching  to  accommodate  new  measurements  until  all  the  USBL  measurements  fit 
within  the  window. 
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The  methodology  for  propagating  the  vehicle  from  the  starting  position  to  the 
zone  where  USBL  measurements  are  available  is  either  the  UKF  or  EKF.  The  MHE  starts 
when  the  USBL  measurements  are  available.  The  threshold  is  set  to  a  small  constant 
value. 


Table  5.  Illustration  of  the  Algorithm  Used  for  the  MHE  with  Epi-Splines 


Input 

XT>  PT’Zk>ZusBL>MHEws 

Output 

%epi’  P epi 

Initialization 

x0,  P0,  MHEWS,  p,  Threshold 

Computation  of  input 

Xj  —  lAnl-Ui+il  ■■■A fc] 

parameters 

PT  =  [Pn\Pn+l\-Pk] 

ZuSBL  ~  iZUSBLn\ZUSBLn+l\  —Z USBLk ] 

Zk  —  [ zn\zn+l  \  ■••zk\ 

nz  —  columns[Zk\ 

Beginning  the  iteration 

with  epi-splines 

if  (nz  >  MHEWS ) 

[xepi>  Pepi\  =  epi_spline(xT.  PT.Zk,ZUSBL ) 

if  (//  <  Threshold ) 

(Xk—N>Xk—N+i’-"Xk)  —  Zepi 

(Pk-N>  Pk-N+l’  ■■■  Pk)T  ~  Pepi 

else 

ZjJSBL  ~  iZUSBLn+l\>  ■"  \ZVSBLk\ 

end  if 

end  if 
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The  optimization  process  takes  place  and  provides  the  estimated  state  as  long  as 
the  optimization  error  is  less  than  the  set  threshold;  otherwise,  the  bad  measurement  is 
left  unused  by  the  optimization,  and  the  window  extends  to  accept  a  new  measurement. 
The  optimization  process  continues  until  all  USBL  measurements  have  been  used.  The 
ability  of  epi-splines  to  incorporate  soft  and  hard  constraints  makes  them  favorites  in 
optimization  problems.  Other  epi-spline  advantages  include  their  applicability  in 
piecewise  functions  of  any  space,  their  requirement  of  one  lower-semicontinuous  input 
boundary,  and  their  ability  to  approximate  every  lower-semicontinuous  function  to  any 
accuracy. 
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V.  RESULTS  AND  ANALYSIS 


A.  METHODOLOGY 

The  EKF,  UKF  and  MHE  with  epi-splines  were  evaluated  on  their  capabilities  of 
estimating  the  position  of  the  REMUS  100  AUV  in  the  future  using  the  current  state  of 
the  system.  Past  docking  mission  runs  of  the  AUV  were  used  to  evaluate  these  position 
estimating  techniques.  To  measure  the  effectiveness  of  each  of  these  techniques,  the  final 
location  of  the  REMUS  100  AUV  was  computed  relative  to  the  center  of  the  entrance  of 
the  docking  station.  The  distance  from  the  center  of  the  docking  station  entrance  to  the 
edge  is  0.35  m  in  all  directions.  If  the  vehicle’s  final  position  is  within  0.35  m  of  the 
center  of  the  entrance,  docking  is  considered  successful.  The  reader  is  referred  to  Figure 
18  for  an  illustration  of  the  docking  error  metric. 

Sometimes  the  USBL  measurements  become  available,  and  sometimes  they  do 
not.  The  USBL  update  rate  is  computed  as  follows: 


USBL  Update  Rate{in  Hz) 


no.  of  USBL  measurementsxaverage  forward  velocity 
distance  between  the  docking  station  and  the  last  waypoint 


In  some  runs,  the  update  rate  is  very  small,  but  the  measurements  are  packed  closer  to  the 
intended  track.  A  total  of  19  previous  REMUS  100  AUV  docking  station  runs  were  used 
for  this  evaluation.  During  these  runs,  the  REMUS  100  AUV  managed  to  dock  only  once 
during  actual  in-water  testing.  The  position  estimating  techniques  were  evaluated  to 
determine  whether  they  would  have  managed  to  dock  the  vehicle  by  placing  within  the 
boundaries  of  the  docking  station  entrance.  The  results  for  this  evaluation  are  shown  in 
the  next  section. 


B.  PREVIOUS  REMUS  100  DOCKING  STATION  RUNS  -  EVALUATION 
RESULTS  AND  ANALYSIS 

Figure  25  shows  the  REMUS  100  AUV’s  dead  reckoning  trajectory  for  an 
underwater  docking  station  run  that  was  conducted  in  Monterey  Bay,  California  in  2014. 
The  vehicle  attempted  to  dock  during  its  first  trial  but  missed  the  docking  station.  The 
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vehicle  made  its  second  attempt  and  docked  successfully.  Shown  in  the  image  is  the 
combination  of  the  two  trajectories  in  blue.  The  red  crosses  represent  the  USBL 
measurements  received  during  the  first  trial,  and  the  black  dots  are  USBL  measurements 
received  during  the  second  trial.  The  vehicle  started  at  the  position  marked  in  green  as 
shown.  The  docking  station  was  close  to  the  starting  point.  The  AUV’s  intended  track 
was  the  straight  cyan  line.  Notably,  the  REMUS  100  AUV  deviates  from  its  path  as  the 
vehicle’s  dead  reckoning  measurement  error  grows  linearly. 
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Figure  25.  MATLAB  Plot  of  REMUS  100  Dead  Reckoning  for  Two  Trials 


The  same  illustration  is  shown  in  Figure  26.  This  diagram  has  been  extracted  from 
the  REMUS  100  AUV  software  (VIP). 
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In  Figure  26,  the  dead  reckoning  trajectory  is  shown  in  blue  arrows;  the  yellow 
arrows  indicate  the  USBL  measurements  available  to  the  vehicle  during  the  two  trials. 
The  docking  station  is  indicated  with  the  cyan  star  at  the  lower  end  of  the  intended  track 
line.  This  docking  station  run  corresponds  with  Runs  16  and  17  in  Table  6. 


Figure  26.  REMUS  100  Dead  Reckoning  Trajectory  from  VIP  Softwear 

Table  6  shows  the  results  of  the  evaluation  conducted  using  previous  docking 
station  runs  of  the  REMUS  100.  As  the  table  indicates,  the  MHE  managed  to  dock  the 
vehicle  on  five  runs  while  the  UKF  and  EKF  only  managed  to  dock  the  vehicle  twice. 
The  UKF  and  EKF  docked  the  vehicle  on  the  same  run  that  the  REMUS  100  managed  to 
dock  during  its  real  mission  in  water.  The  MHE  did  not  dock  on  this  run,  but  it  placed  the 
vehicle  close  to  the  docking  station. 
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Table  6.  Results  Obtained  from  Previous  REMUS  100  Mission  Runs  Performed 

Using  the  UKF,  EKF  and  MHE 


Runs 

USBL 

Update  rate 

Initial 

Uncertainty 

UKF 

EKF 

MHE 

Docking 

Success 

(Hz) 

(m) 

Docking 
error  (m) 

Docking 
error  (m) 

Docking 
error  (m) 

REMUS 

UKF 

EKF 

MHE 

1 

0.401 

20.80 

3.93 

4.65 

0.46 

No 

No 

No 

No 

2 

0.299 

16.00 

8.52 

9.22 

0.58 

No 

No 

No 

No 

3 

0.319 

22.40 

3.26 

4.51 

0.46 

No 

No 

No 

No 

4 

0.428 

15.80 

4.20 

5.40 

0.18 

No 

No 

No 

Yes 

5 

0.436 

13.70 

4.23 

4.87 

0.08 

No 

No 

No 

Yes 

6 

0.252 

8.20 

3.38 

3.84 

0.31 

No 

No 

No 

Yes 

7 

0.209 

2.50 

5.53 

6.32 

4.82 

No 

No 

No 

No 

8 

0.351 

0.00 

3.09 

4.01 

0.98 

No 

No 

No 

No 

9 

0.255 

2.60 

3.24 

4.41 

1.22 

No 

No 

No 

No 

10 

0.343 

0.00 

6.79 

7.20 

0.43 

No 

No 

No 

No 

11 

0.429 

3.70 

2.48 

3.38 

0.07 

No 

No 

No 

Yes 

12 

0.346 

6.80 

1.53 

2.76 

0.44 

No 

No 

No 

No 

13 

0.344 

4.60 

2.22 

2.96 

0.79 

No 

No 

No 

No 

14 

0.307 

5.80 

1.96 

3.35 

0.28 

No 

No 

No 

Yes 

15 

0.373 

8.10 

1.37 

3.20 

1.73 

No 

No 

No 

No 

16 

0.285 

9.10 

0.23 

0.28 

0.50 

Yes 

Yes 

Yes 

No 

17 

0.501 

7.50 

0.72 

0.53 

0.43 

No 

No 

No 

No 

18 

0.346 

15.10 

1.27 

0.07 

2.70 

Yes 

No 

Yes 

No 

19 

0.283 

20.00 

0.13 

0.50 

1.60 

Yes 

Yes 

No 

No 

Average 

0.336 

9.62 

3.06 

3.76 

0.95 

The  average  docking  error  associated  with  the  MHE  is  relatively  small  compared 
to  the  EKF  and  UKF. 

In  Run  2,  the  vehicle  position  was  estimated  very  far  from  the  docking  station 
using  UKF  and  EKF  techniques,  but  the  MHE  located  it  about  0.58  m  away  from  the 
docking  station.  The  depiction  of  this  run  is  shown  in  Figure  27.  The  UKF  and  EKF 
followed  the  position  of  USBL  measurements  with  small  consideration  of  the  vehicle 
dynamics  due  to  the  tuning  of  the  covariance  associated  with  the  system  model  and  the 
covariance  associated  with  the  measurement  model. 
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Figure  27.  Run  2  Illustrating  the  Worse  Performance  by  the  UKF,  EKF,  and  FBS 

Techniques 

In  Run  3,  the  vehicle  had  high  initial  uncertainty  about  the  USBL  measurements, 
but  the  perfonnance  from  all  techniques  was  better  than  some  of  the  runs  with  low  levels 
of  initial  uncertainty.  Figure  28  shows  the  performance  of  these  techniques  under  the 
vehicle’s  high  uncertainty  about  the  USBL  measurements. 
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Figure  28.  Run  3  Illustrating  UKF,  EKF  and  MHE  Performance  at  a  High  Level 

of  Initial  Uncertainty 

In  Run  4,  the  USBL  sensor  update  rate  is  the  highest  for  the  runs  during  which  the 
REMUS  vehicle  did  not  dock.  Analysis  finds  that  the  vehicle  could  have  docked  if  it  had 
been  guided  by  the  MHE  technique.  Figure  29  depicts  this  finding. 


56 


Run  14  had  a  low  frequency  update  rate,  but  MHE  technique  managed  to  dock. 
Figure  30  illustrates  this  docking.  Once  again,  the  EKF  and  UKF  tracked  the  USBL 
measurements  with  small  consideration  of  the  vehicle  dynamics  due  to  the  tuning  of  the 
covariance  associated  with  the  system  model  and  the  covariance  associated  with  the 
measurement  model. 
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In  Run  16,  the  REMUS  vehicle  docked,  which  means  the  USBL  measurements 
were  good  and  directed  the  vehicle  into  the  docking  station.  This  is  confirmed  by  docking 
with  the  UKF  and  EKF  because  they  are  good  at  tracking  the  measurements.  In  this  run, 
the  MHE  did  not  dock.  This  was  expected  because  MHE  considers  the  dynamics  of  the 
vehicle;  this  time,  the  MHE  techniques  failed,  but  managed  to  place  the  vehicle  within 
0.5m  from  the  center  of  the  docking  station.  The  equation  used  to  combine  the  effect  of 

the  vehicle  dynamics  and  the  USBL  measurements  is  as  follows: 

2  2 
aDRymhe  °USBL,yDR 

y optml  mhe  ~  ~2  2  *"  T2  T“72 

UDR  "■  uUSBL  UDR  '  uUSBL 

In  (46)  y optml  mhe  is  the  East  (y)  coordinate  of  the  position  of  the  vehicle  as  computed  by 
the  MHE  with  epi-splines  method,  <jdr  is  the  uncertainty  associated  with  the  dead 
reckoning  measurements  at  that  specific  location,  ymhe  is  the  output  of  the  MHE  with 

epi-splines  method  before  considering  the  covariance  associated  with  the  dead  reckoning 
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measurements  and  with  the  USBL  measurements.  In  (46)  yDR  is  the  East  (y)  component 
of  the  dead  reckoning  position  of  the  vehicle  while  <JUSBL  is  the  covariance  associated 
with  the  USBL  measurements.  The  covariance  associated  with  the  dead  reckoning 
measurements  grows  linearly  with  the  distance  travelled  while  the  covariance  associated 
with  the  USBL  measurements  decreases  linearly  as  the  vehicle  gets  closer  to  the  docking 
station  but  cannot  be  zero  because  the  USBL  measurements  normally  end  at  about  5  m 
away  from  the  docking  station. 

As  the  vehicle  moves  closer  to  the  docking  station  the  yoptmi  mhe  tends  to  move 
away  from  the  dead  reckoning  position  of  the  vehicle  due  to  (46).  This  becomes  a 
challenge  to  the  MHE  especially  when  the  USBL  measurements  are  on  the  intended 
track;  the  MHE  with  epi-spline  y0ptmi  mhe  tends  to  be  a  little  off  from  the  intended  track 
unlike  the  EKF  and  UKF.  Figures  31  and  32  provide  the  illustration  of  this  situation.  In 
this  image,  the  measurements  are  on  the  intended  track  as  a  result,  EKF  and  UKF 
managed  to  dock  but  MHE  with  epi-spline  went  a  little  off  from  the  docking  station. 
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In  Run  19,  the  REMUS  100  AUV  docked  successfully.  The  UKF  also  managed  to 
dock  as  expected.  As  discussed  in  Run  16’s  explanation,  the  MHE  with  epi-splines 
missed  the  docking  station  by  a  small  error.  Figure  32  shows  the  distribution  of  the  USBL 
measurements  and  the  final  estimated  positions  of  the  vehicle  for  other  position 
estimating  techniques. 
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VI.  POSITION  ESTIMATION  USING  FBS 


A.  INTRODUCTION 

After  evaluating  the  EKF  and  the  UKF  position  estimating  methods  and  obtaining 
unsatisfactory  results,  the  forward  and  backward  smoothing  (FBS)  position  estimating 
method  was  introduced.  The  FBS  is  a  technique  applied  to  the  measurements  to  obtain  an 
optimal  smoothed  estimate  [23].  “The  information  filter  is  defined  as  the  implementation 
of  the  Kalman  filter  that  propagates  the  inverse  of  the  covariance  instead  of  propagating 
the  covariance”  [23].  Mathematically,  the  information  filter  is  related  to  the  Kalman  filter 
by 

knf  =  P~X  (47) 

In  (47)  Iinf  is  the  certainty  in  the  state  for  the  infonnation  filter  and  P_1  is  the  inverse  of 
the  state  covariance  for  the  forward  smoothing  techniques  [23].  The  information  filter  is 
recommended  for  use  in  the  backward  smoothing  technique  where  the  knowledge  of  the 
state  is  uncertain  or  unavailable.  This  is  due  to  the  fact  that  the  information  filter  is 
mathematically  defined  with  zero  confidence,  I[nf  —  0,  where  the  uncertainty  of  the 
forward  smoothing  algorithm  approaches  infinity,  Pk  oo  [23].  The  equations  used  to 
implement  the  backward  smoothing  as  detailed  in  [23]  are  as  follows: 

Iinf'  =  Q -1  -  (jinfk_1  +  0k rQ~10ky1  0k  Q~X  (48) 

I  inf  l  is  the  certainty  associated  with  the  state  at  the  previous  time  step. 

Q-1  is  the  inverse  covariance  for  the  process. 

0k  is  the  continuous  state  transition  matrix  as  defined  by  0  =  eAdt . 

Imfk_i  is  the  certainty  associated  with  the  state  at  the  previous  time  step. 

hnfl  =  Iinf-  +  HkR^Hk  (49) 

hnfk  is  the  certainty  associated  with  the  state  at  one  time  step  into  the  future. 

Hi  is  the  transpose  of  the  Jacobian  obtained  by  linearizing  the  measurement  function  h 
around  the  current  state. 
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R  1  is  the  inverse  covariance  for  the  measurements. 


Kk  =  (linflY  Hill-1 

(50) 

Kk  is  the  Kalman  gain. 

*k  =  <Mfc+-i 

(51) 

xk  is  the  predicted  state. 

xk_t  is  the  state  at  previous  time  step. 

A  =  xf  +  Kk(Zk  -  HkxrJ  (52) 

xk  is  the  smoothed  estimate  at  one  time  step  into  the  future. 


Zk  is  the  observed  state. 


The  FBS  technique  results  in  two  optimal  estimates.  The  first  estimate  is  obtained 
using  UKF  running  forward  in  time  from  the  first  to  the  fifth  USBL  measurements  in  the 
interval  of  ten  measurements.  The  second  estimate  is  obtained  using  an  information  filter 
running  backward  in  time  from  the  last  USBL  measurement  in  the  current  window  to  the 
fifth  measurement  in  the  current  window  size  for  the  first  nine  USBL  measurements.  The 
two  estimates  are  combined  according  to  their  Kalman  gains  and  their  covariances.  The 
equations  used  are  as  follows: 

Pm  =  [( PfmY 1  +  (^m)-1]"1  ^ 

Pm  is  the  optimal  smoothed  estimate  covariance. 

_ 

(p/J  is  the  inverse  of  the  forward  smoothed  estimate  which  is  equal  to  (Pfe)  1 . 

(Pbm)1  is  the  inverse  of  the  backward  smoothed  estimate  which  is  equal  to  hnfk- 

PrrSjfmP^fm  4”  ^bm^bm)  (54) 

xm  is  the  optimal  smoothed  estimate. 


Ifm  is  the  certainty  of  the  forward  smoothing  technique. 


Ibm  is  the  certainty  of  the  backward  smoothing  technique. 
Xfm  is  the  forward  smoothed  estimate. 
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xbm  is  the  backward  smoothed  estimate. 

The  resultant  smoothed  estimate  was  utilized  as  an  optimal  estimate  by  an 
ordinary  UKF  that  updated  the  state  using  only  the  smoothed  estimates.  The  window  of 
measurements  expanded  as  new  USBL  measurements  were  received  until  all  the 
available  measurements  were  used  to  estimate  the  state  of  the  system.  Figure  33 
illustrates  how  the  FBS  was  implemented.  This  image  shows  an  uncertainty  associated 
with  the  USBL  measurements  at  the  start  of  the  region  where  the  vehicle  receives  USBL 
measurements.  The  image  also  shows  an  uncertainty  associated  with  the  starting  point  of 
the  backward  smoothing  as  well  as  the  red  dot  representing  a  smoothed  estimate.  The 
image  also  shows  the  window  with  9  USBL  measurements.  Two  REMUS  100  AUV 
positions  are  shown,  representing  a  solution  calculated  from  the  dead  reckoning 
measurements  only,  and  the  solution  calculated  from  the  FBS-smoothed  estimates. 


The  algorithm  used  for  the  FBS  technique  is  illustrated  in  Table  7.  In  this  table,  N 
is  the  number  of  USBL  measurements  and  m  —  N/2  is  the  stop  position  for  both 
techniques. 
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Table  7.  FBS  Technique  Algorithm 


Information  Filter  Input 

xk,hnfk  'Zk'N  k  —  N:m 

UKF  Input 

xk\k-i'Pk\k-i>Zk’N  k  =  l:m 

Information  Filter  Output 

1 bm>  % bm 

UKF  Output 

1+  y  + 

■A'fm 

FBS  output 

Information  Filter  initialization 

Xk-lJlnfl^m 

UKF  initialization 

xk_r>Pk_Vtm 

Core  steps 

for  (fc  =  1:  m) 

\jfm>  Xfm]  UPP  (,xk\k—l*  Pk\k—1  i  Zf) 

end  for 

for  ( k  =  N:  rn) 

Uhm’ Xhm]  =  Inf o _filter (xk,Iinf~k  ,Zkl) 

end  for 

Pfm  =  OfmY1 

Pbm  Obm) 

Pm  =  [(P/mT1  +  (Pfm)-1]"1 

xm  ~~  PmQfmxfm  P  ^ bmxbm ) 

Input  to  standard  UKF 

xk\k-l’  Pk\k-1  ’  Zk  DR 

Output  from  standard  UKF 

xk,  Pk  and  xm,  Pm  when  available 

B.  FBS  EVALUATION  AND  COMPARISON  WITH  UKF,  EKF  AND  MHE 
RESULTS 

The  results  associated  with  the  FBS  technique  are  shown  in  Table  8.  The 
evaluation  of  the  FBS  was  similar  to  the  one  conducted  on  the  MHE  with  epi-splines.  The 
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results  obtained  using  the  FBS  are  compared  to  the  results  obtained  using  the  UKF,  EKF 
and  the  MHE  with  epi-splines.  It  can  be  noted  that  the  FBS  managed  to  dock  the  AUV 
five  times  as  shown  on  the  highlighted  cells.  The  FBS  performed  much  better  than  the 
UKF  and  EKF  methods. 


Table  8.  Docking  Station  Results  for  FBS  in  Comparison  with  UKF,  EKF 

and  MHE  Methods 


Runs 

USBL 

Update  rate 

Initial 

Uncertainty 

UKF 

EKF 

FBS 

MHE 

Docking 

Success 

(Hz) 

(m) 

Docking 
error  (m) 

Docking 
error  (m) 

Docking 
error  (m) 

Docking 
error  (m) 

REMUS 

UKF 

EKF 

FBS 

MHE 

1 

0.401 

20.80 

3.93 

4.65 

2.79 

0.46 

No 

No 

No 

No 

No 

2 

0.299 

16.00 

8.52 

9.22 

6.00 

0.58 

No 

No 

No 

No 

No 

3 

0.319 

22.40 

3.26 

4.51 

2.17 

0.46 

No 

NO 

NO 

NO 

No 

4 

0.428 

15.80 

4.20 

5.40 

3.04 

0.18 

NO 

NO 

NO 

NO 

Yes 

5 

0.436 

13.70 

4.23 

4.87 

3.01 

0.08 

No 

No 

No 

No 

Yes 

6 

0.252 

8.20 

3.38 

3.84 

1.69 

0.31 

No 

No 

No 

No 

Yes 

7 

0.209 

2.50 

5.53 

6.32 

4.10 

4.82 

No 

No 

No 

No 

No 

8 

0.351 

0.00 

3.09 

4.01 

0.23 

0.98 

No 

No 

No 

Yes 

No 

9 

0.255 

2.60 

3.24 

4.41 

2.15 

1.22 

No 

No 

No 

No 

No 

10 

0.343 

0.00 

6.79 

7.20 

5.73 

0.43 

No 

NO 

No 

No 

No 

11 

0.429 

3.70 

2.48 

3.38 

0.33 

0.07 

NO 

NO 

NO 

Yes 

Yes 

12 

0.346 

6.80 

1.53 

2.76 

1.08 

0.44 

NO 

NO 

NO 

No 

NO 

13 

0.344 

4.60 

2.22 

2.96 

0.76 

0.79 

No 

No 

No 

No 

No 

14 

0.307 

5.80 

1.96 

3.35 

0.30 

0.28 

No 

No 

No 

Yes 

Yes 

15 

0.373 

8.10 

1.37 

3.20 

1.33 

1.73 

No 

No 

No 

No 

No 

16 

0.285 

9.10 

0.23 

0.28 

0.46 

0.50 

Yes 

Yes 

Yes 

No 

No 

17 

0.501 

7.50 

0.72 

0.53 

0.32 

0.43 

No 

No 

No 

Yes 

No 

18 

0.346 

15.10 

1.27 

0.07 

0.20 

2.70 

Yes 

NO 

Yes 

Yes 

No 

19 

0.283 

20.00 

0.13 

0.50 

0.90 

1.60 

Yes 

Yes 

NO 

No 

NO 

Average 

0.336 

9.62 

3.06 

3.76 

1.93 

0.95 

The  average  docking  error  associated  with  FBS  is  1.93  m  which  is  much  less  than 
the  docking  error  obtained  using  UKF  and  EKF  methods.  Meanwhile,  the  MHE 
algorithm  successfully  docked  the  vehicle  as  many  times  as  the  FBS  algorithm  did. 

Figure  34  is  Run  2  in  Table  8.  This  figure  depicts  a  very  bad  run  with  badly 
distributed  USBL  measurements.  The  FBS  performed  better  than  UKF  and  EKF  on  this 
run. 
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Figure  34.  Run  2  with  Poorly  Distributed  USBL  Measurements 


In  Run  14,  the  FBS  had  a  successful  docking  at  a  low  USBL  measurement  update 
rate.  Figure  35  illustrates  the  distribution  of  the  position  estimates  obtained  by  the  FBS 
technique  and  other  methods  analyzed  during  this  run. 
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The  FBS  results  demonstrate  a  system  that  does  not  entirely  depend  on  the  sparse, 
inconsistent  and  erroneous  USBL  measurements.  The  results  indicate  that  FBS  maintains 
a  minimum  covariance  while  also  trying  to  reflect  the  vehicle  dynamics.  The  strength  of 
the  FBS  technique  is  the  use  of  an  optimal  smoothed  estimate  to  update  the  position  of 
the  AUV.  The  weakness  associated  with  the  FBS  is  the  initialization  requirement;  one 
needs  to  have  some  knowledge  of  the  actual  position  of  the  AUV  in  order  to  allocate  the 
initial  position  for  the  backward  smoothing  process,  since  this  method  can  fail  to 
converge  when  initialized  too  far  from  the  true  initial  position.  Another  weakness 
associated  with  the  FBS  is  the  fact  that  it  does  not  use  all  the  USBL  measurements,  this 
poses  a  risk  of  not  using  the  best  measurements. 


69 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


70 


VII.  CONCLUSION  AND  FUTURE  WORK 


A.  CONCLUSION 

The  purpose  of  this  study  was  to  develop  a  methodology  for  position  estimation 
by  which  sparse,  erroneous,  and  inconsistent  sensor  observations  can  be  used  for 
autonomous  underwater  vehicle  (AUV)  terminal  homing  to  an  undersea  docking  station. 

An  undersea  docking  station  offers  great  potential  for  increasing  at-sea  time  and 
reducing  survey  costs  of  AUVs.  A  key  part  of  this  system  is  the  AUV’s  ability  to 
successfully  dock.  Part  of  the  process  for  successful  docking  is  estimating  the  AUV’s 
position.  The  AUV’s  D-USBL  sensor  system  provides  range  and  bearing  measurements 
to  the  docking  station.  The  D-USBL  range  and  bearing  measurements  are  nonlinear, 
sparse,  erroneous  and  inconsistent. 

Position  estimating  methods,  such  as  EKF  and  UKF  were  first  considered  and 
evaluated  since  the  system  is  nonlinear  in  range  and  bearing  measurements  and  these  are 
estimation  techniques  for  nonlinear  systems.  The  evaluation  results  showed  relatively 
poor  performance  for  these  filters. 

The  MHE  with  epi-splines  was  then  introduced  as  an  alternative  for  position 
estimation  for  the  AUV  to  an  undersea  docking  station.  The  MHE  smoothing  filter  used 
the  dead  reckoning  measurements  and  the  D-USBL  measurements  as  constraints  for  the 
epi-splines  optimization  method. 

An  analysis  based  on  data  sets  of  REMUS  100  AUV  docking  station  runs  was 
conducted  using  the  MHE  with  epi-splines  methodology  and  compared  to  EKF  and  UKF 
algorithms.  The  MHE  with  epi-splines  algorithm  demonstrated  significantly  better 
performance  over  the  EKF  and  UKF. 

A  further  analysis  with  FBS  was  also  conducted.  The  FBS  technique  was 
evaluated  in  the  same  manner  as  the  MHE  with  epi-splines  technique,  and  compared  with 
UKF,  EKF  and  MHE  with  epi-splines  position  estimating  methods.  The  FBS  method 
produced  better  results  than  the  UKF  and  EKF  methods,  but  the  MHE  remained  the  best 
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method;  it  produced  consistently  better  docking  results  on  real  underwater  docking 
datasets. 

The  EKF  had  the  highest  average  docking  error  of  3.76  m.  The  average  docking 
error  associated  with  UKF  was  3.06  m.  FBS  technique  had  an  average  docking  error  of 
1.93  m  while  the  MHE  with  epi-splines  had  an  average  error  of  0.95  m.  It  was  expected 
that  the  EKF  would  have  the  highest  average  error  due  to  its  performance  limitations 
when  applied  to  systems  which  are  highly  nonlinear. 

The  lowest  average  docking  error  associated  with  the  MHE  indicates  the  great 
potential  that  the  MHE  has  for  guiding  an  AUV  to  an  undersea  docking  station  using 
sparse,  erroneous  and  inconsistent  USBL  measurements. 

B.  FUTURE  WORK 

The  opportunity  to  evaluate  all  these  position  estimating  techniques  in  a  real 
underwater  docking  environment  was  not  available  for  this  thesis.  Examining  the  MHE 
with  epi-splines  in  a  real  underwater  docking  environment  could  produce  many 
successful  REMUS  100  AUV  dockings. 

The  possibilities  of  utilizing  the  UKF,  EKF  and  FBS  with  a  secondary  in-close 
sensor  system  should  be  considered.  The  UKF,  EKF  and  FBS  results  showed  that  they 
are  good  at  tracking  the  USBL  measurements,  but  these  measurements  (in  many  cases) 
end  when  the  vehicle  gets  within  5  m  of  the  docking  station.  As  a  result,  these  position 
estimating  techniques  only  guide  the  AUV  to  a  position  about  5  m  away  from  the 
docking  station.  A  secondary  in-close  sensor  system  could  provide  a  good  solution  to  this 
problem. 

Investigation  on  when  to  ignore  USBL  measurements  should  be  considered  for 
future  studies.  Some  USBL  measurements  lie  far  from  the  region  where  most  USBL 
measurements  are  concentrated.  These  outliers  cannot  realistically  reflect  the  actual 
vehicle  position,  due  to  the  vehicle’s  dynamic  limitations.  In  this  thesis,  these 
measurements  were  utilized  like  other  USBL  measurements,  but  developing  a  method  for 
eliminating  spurious  USBL  measurements  could  produce  better  docking  results. 
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Development  of  a  minimal  measurement  characteristic  (in  tenns  of  sparsity, 
errors  and  inconsistency  of  the  USBL  measurements)  for  successful  docking  should  also 
be  considered.  Some  sets  of  measurements  are  much  more  sparse,  erroneous  or 
inconsistent  than  other  sets.  Having  a  methodology  for  determining  whether  the  USBL 
measurements  are  good  or  poor  will  assist  in  guiding  the  AUV  since  poor  USBL 
measurements  will  be  rejected. 
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